void packetHandler(int packetID, int readlength)
{
static char data[4096];
// static int bytesRead = 0; // Not used
// Wifi_RxRawReadPacket: Allows user code to read a packet from within the WifiPacketHandler function
// long packetID: a non-unique identifier which locates the packet specified in the internal buffer
// long readlength: number of bytes to read (actually reads (number+1)&~1 bytes)
// unsigned short * data: location for the data to be read into
// bytesRead = Wifi_RxRawReadPacket(packetID, readlength, (unsigned short *)data); // Not used
Wifi_RxRawReadPacket(packetID, readlength, (unsigned short *)data);
// Check this is the right kind of packet
if (data[32] == 'Y' && data[33] == 'O') {
u8 command = data[34];
u8 val = data[35];
int sendid = *((int*)(data+36));
if (lastSendid == sendid) {
if (!(ioRam[0x02] & 0x01)) {
nifiSendid--;
sendPacketByte(56, linkSendData);
nifiSendid++;
}
return;
}
lastSendid = sendid;
if (command == 55 || command == 56) {
//printLog("%d: Received %x\n", ioRam[0x02]&1, val);
linkReceivedData = val;
}
//linkReceivedData = 0;
switch(command) {
// Command sent from "internal clock"
case 55:
if (ioRam[0x02] & 0x80) {
// Falls through to case 56
}
else {
printLog("Not ready!\n");
transferWaiting = true;
timerStart(2, ClockDivider_64, 10000, transferWaitingTimeoutFunc);
break;
}
// Internal clock receives a response from external clock
case 56:
transferReady = true;
cyclesToExecute = -1;
nifiSendid++;
break;
default:
//printLog("Unknown packet\n");
break;
}
}
}
int main(int argc, const char *argv[]){
ClientServerHello *client_hello, *server_hello;
Handshake *handshake, *server_handshake;
RecordLayer *record, *server_message, *temp_record, record2;
ClientKeyExchange *client_key_exchange;
ServerKeyExchange *server_key_exchange;
Certificate *certificate;
CipherSuite *ciphersuite_choosen;
Finished finished;
Talker sender;
int pre_master_secret_size, phase, key_block_size, enc_message_len, dec_message_len;
unsigned int supported_ciphers;
uint8_t **pre_master_secret;
MD5_CTX md5;
SHA_CTX sha;
uint8_t *enc_message, *dec_message, *mac, *key_block, *client_write_MAC_secret, *server_write_MAC_secret, *master_secret, *sha_1, *md5_1, *sha_fin, *md5_fin, *mac_test;
client_hello = NULL;
server_hello = NULL;
handshake = NULL;
server_handshake = NULL;
record = NULL;
server_message = NULL;
temp_record = NULL;
client_key_exchange = NULL;
server_key_exchange = NULL;
certificate = NULL;
ciphersuite_choosen = NULL;
pre_master_secret_size = 0;
key_block_size = 0;
enc_message_len = 0;
dec_message_len = 0;
pre_master_secret = NULL;
phase=0;
enc_message = NULL;
dec_message = NULL;
mac = NULL;
key_block = NULL;
client_write_MAC_secret = NULL;
server_write_MAC_secret = NULL;
master_secret = NULL;
sha_1 = NULL;
md5_1 = NULL;
sha_fin = NULL;
md5_fin = NULL;
mac_test = NULL;
sender = client;
SHA1_Init(&sha);
MD5_Init(&md5);
///////////////////////////////////////////////////////////////PHASE 1//////////////////////////////////////////////////////////
OpenCommunication(client);
printf("Insert your favorite cipher:");
scanf("%x", &supported_ciphers);
if (supported_ciphers < 0x03 || (0xB <= supported_ciphers && supported_ciphers <= 0x10) || supported_ciphers>=0x17){
perror("Not valid ciphersuite inserted.");
exit(1);
}
printf("\n\n");
uint8_t code = (uint8_t)supported_ciphers;
client_hello = ClientServerHello_init(CLIENT_HELLO, 0, &code, 1);
//Wrapping
handshake = ClientServerHelloToHandshake(client_hello);
record = HandshakeToRecordLayer(handshake);
//Sending client hello
sendPacketByte(record);
printRecordLayer(record);
SHA1_Update(&sha, record->message, sizeof(uint8_t)*(record->length-5));
MD5_Update(&md5, record->message, sizeof(uint8_t)*(record->length-5));
FreeRecordLayer(record);
FreeHandshake(handshake);
//Opening the communication to the server and, when authorized, reading server hello
OpenCommunication(server);
while(CheckCommunication() == server){}
server_message = readchannel();
printRecordLayer(server_message);
server_handshake = RecordToHandshake(server_message);
server_hello = HandshakeToClientServerHello(server_handshake);
SHA1_Update(&sha, server_message->message, sizeof(uint8_t)*(server_message->length-5));
MD5_Update(&md5, server_message->message, sizeof(uint8_t)*(server_message->length-5));
FreeRecordLayer(server_message);
FreeHandshake(server_handshake);
ciphersuite_choosen = CodeToCipherSuite(server_hello->ciphersuite_code[0]);
OpenCommunication(server);
phase = 2;
///////////////////////////////////////////////////////////////PHASE 2//////////////////////////////////////////////////////////
while(phase == 2){
while(CheckCommunication() == server){}
server_message = readchannel();
printRecordLayer(server_message);
server_handshake = RecordToHandshake(server_message);
switch (server_handshake->msg_type) {
case CERTIFICATE:
certificate = HandshakeToCertificate(server_handshake);
SHA1_Update(&sha,server_message->message, sizeof(uint8_t)*(server_message->length-5));
MD5_Update(&md5,server_message->message, sizeof(uint8_t)*(server_message->length-5));
FreeRecordLayer(server_message);
FreeHandshake(server_handshake);
OpenCommunication(server);
break;
case SERVER_KEY_EXCHANGE:
server_key_exchange = HandshakeToServerKeyExchange(server_handshake, certificate);
SHA1_Update(&sha, server_message->message, sizeof(uint8_t)*(server_message->length-5));
MD5_Update(&md5, server_message->message, sizeof(uint8_t)*(server_message->length-5));
Verify_(ciphersuite_choosen, client_hello, server_hello, server_key_exchange->parameters, server_key_exchange->len_parameters, server_key_exchange->signature, server_key_exchange->len_signature, certificate);
FreeRecordLayer(server_message);
FreeHandshake(server_handshake);
OpenCommunication(server);
break;
case SERVER_DONE:
SHA1_Update(&sha, server_message->message, sizeof(uint8_t)*(server_message->length-5));
MD5_Update(&md5, server_message->message, sizeof(uint8_t)*(server_message->length-5));
FreeRecordLayer(server_message);
FreeHandshake(server_handshake);
phase = 3;
break;
default:
perror("ERROR: Unattended message in phase 2.\n");
exit(1);
break;
}
}
///////////////////////////////////////////////////////////////PHASE 3//////////////////////////////////////////////////////////
while(phase == 3){
///CLIENT_KEY_EXCHANGE///
pre_master_secret = (uint8_t**)calloc(1, sizeof(uint8_t*));
client_key_exchange = ClientKeyExchange_init(ciphersuite_choosen, certificate, server_key_exchange, pre_master_secret, &pre_master_secret_size);
handshake = ClientKeyExchangeToHandshake(client_key_exchange);
record = HandshakeToRecordLayer(handshake);
sendPacketByte(record);
printRecordLayer(record);
OpenCommunication(server);
SHA1_Update(&sha,record->message, sizeof(uint8_t)*(record->length-5));
MD5_Update(&md5,record->message, sizeof(uint8_t)*(record->length-5));
FreeClientKeyExchange(client_key_exchange);
FreeServerKeyExchange(server_key_exchange);
FreeCertificate(certificate);
FreeRecordLayer(record);
FreeHandshake(handshake);
//MASTER KEY COMPUTATION
printf("PRE MASTER:\n");
for (int i = 0; i<pre_master_secret_size; i++) {
printf("%02X ", (*pre_master_secret)[i]);
}
printf("\n");
master_secret = MasterSecretGen(*pre_master_secret, pre_master_secret_size, client_hello, server_hello);
free(*pre_master_secret);
free(pre_master_secret);
//free(&pre_master_secret);//TODO:rivedere
//TODO: rimuovere questi print
printf("MASTER KEY:generated\n");
for (int i=0; i< 48; i++){
printf("%02X ", master_secret[i]);
}
printf("\n\n");
//KEYBLOCK GENERATION
key_block = KeyBlockGen(master_secret, ciphersuite_choosen, &key_block_size, client_hello, server_hello);
printf("KEY BLOCK\n");
for (int i=0; i< key_block_size; i++){
printf("%02X ", key_block[i]);
}
printf("\n\n");
phase = 4;
FreeClientServerHello(client_hello);
FreeClientServerHello(server_hello);
}
///////////////////////////////////////////////////////////////PHASE 4//////////////////////////////////////////////////////////
while(CheckCommunication() == server){};
record = ChangeCipherSpecRecord();
sendPacketByte(record);
printRecordLayer(record);
FreeRecordLayer(record);
OpenCommunication(server);
while(CheckCommunication() == server){};
//building finished
SHA1_Update(&sha, &sender, sizeof(uint32_t));
MD5_Update(&md5, &sender, sizeof(uint32_t));
SHA1_Update(&sha,master_secret,sizeof(uint8_t)*48);
MD5_Update(&md5,master_secret,sizeof(uint8_t)*48);
SHA1_Update(&sha, pad_1, sizeof(uint8_t)*40);
MD5_Update(&md5, pad_1, sizeof(uint8_t)*48);
md5_1 = calloc(16, sizeof(uint8_t));
sha_1 = calloc(20, sizeof(uint8_t));
SHA1_Final(sha_1,&sha);
MD5_Final(md5_1,&md5);
SHA1_Init(&sha);
MD5_Init(&md5);
SHA1_Update(&sha, master_secret,sizeof(uint8_t)*48);
SHA1_Update(&sha, pad_2,sizeof(uint8_t)*40);
SHA1_Update(&sha, sha_1,sizeof(uint8_t)*20);
MD5_Update(&md5, master_secret,sizeof(uint8_t)*48);
MD5_Update(&md5, pad_2,sizeof(uint8_t)*48);
MD5_Update(&md5, md5_1,sizeof(uint8_t)*16);
md5_fin = calloc(16, sizeof(uint8_t));
sha_fin = calloc(20, sizeof(uint8_t));
SHA1_Final(sha_fin, &sha);
MD5_Final(md5_fin, &md5);
//Finished allocation:
finished.hash = (uint8_t*)calloc(36, sizeof(uint8_t));
memcpy(finished.hash, md5_fin, 16*sizeof(uint8_t));
memcpy(finished.hash + 16, sha_fin, 20*sizeof(uint8_t));
free(sha_1);
free(md5_1);
free(md5_fin);
free(sha_fin);
/* MAC and ENCRYPTION*/
handshake = FinishedToHandshake(&finished);
temp_record = HandshakeToRecordLayer(handshake);
free(finished.hash);
//compute MAC
client_write_MAC_secret = NULL;
server_write_MAC_secret = NULL;
client_write_MAC_secret = key_block;
mac = MAC(ciphersuite_choosen, handshake, client_write_MAC_secret);
uint8_t *message_with_mac;
message_with_mac = (uint8_t*)calloc(temp_record->length + ciphersuite_choosen->hash_size - 5, sizeof(uint8_t));
memcpy(message_with_mac, temp_record->message, temp_record->length);
memcpy(message_with_mac + (temp_record->length - 5) , mac, ciphersuite_choosen->hash_size);
free(mac);
// update length
temp_record->length = temp_record->length + ciphersuite_choosen->hash_size;
free(temp_record->message);
temp_record->message = message_with_mac;
uint8_t length_bytes[4];
int_To_Bytes(temp_record->length, length_bytes);
//TODO: temp_record to record
printf("FINISHED:to sent\n");
printf("%02X ", temp_record->type);
printf("%02X ", temp_record->version.major);
printf("%02X ", temp_record->version.minor);
printf("%02X ", length_bytes[2]);
printf("%02X ", length_bytes[3]);
for(int i=0; i<temp_record->length - 5; i++){
printf("%02X ", temp_record->message[i]);
}
printf("\n\n");
enc_message = DecEncryptPacket(temp_record->message, temp_record->length - 5, &enc_message_len, ciphersuite_choosen, key_block, client, 1);
FreeRecordLayer(temp_record);
FreeHandshake(handshake);
record2.type = HANDSHAKE;
record2.length = enc_message_len + 5;
record2.version = std_version;
record2.message = enc_message;
sendPacketByte(&record2);
int_To_Bytes(record2.length, length_bytes);
printf("ENCRYPED FINISHED: sent\n");
printf("%02X ", record2.type);
printf("%02X ", record2.version.major);
printf("%02X ", record2.version.minor);
printf("%02X ", length_bytes[2]);
printf("%02X ", length_bytes[3]);
for(int i=0; i<record2.length - 5; i++){
printf("%02X ", record2.message[i]);
}
printf("\n\n");
free(record2.message);
OpenCommunication(server);
while(CheckCommunication() == server){};
//CHANGE CIPHER SPEC
server_message = readchannel();
printRecordLayer(server_message);
OpenCommunication(server);
while(CheckCommunication() == server){};
FreeRecordLayer(server_message);
//FINISHED
server_message = readchannel();
int_To_Bytes(server_message->length, length_bytes);
printf("FINISHED ENCRYPED: received\n");
printf("%02X ", server_message->type);
printf("%02X ", server_message->version.major);
printf("%02X ", server_message->version.minor);
printf("%02X ", length_bytes[2]);
printf("%02X ", length_bytes[3]);
for(int i=0; i<server_message->length - 5; i++){
printf("%02X ", server_message->message[i]);
}
printf("\n\n");
dec_message = DecEncryptPacket(server_message->message, server_message->length - 5, &dec_message_len, ciphersuite_choosen, key_block, server, 0);
int_To_Bytes(dec_message_len + 5, length_bytes);
printf("FINISHED DECRYPTED\n");
printf("%02X ", server_message->type);
printf("%02X ", server_message->version.major);
printf("%02X ", server_message->version.minor);
printf("%02X ", length_bytes[2]);
printf("%02X ", length_bytes[3]);
for(int i = 0; i < dec_message_len; i++){
printf("%02X ", dec_message[i]);
}
printf("\n\n");
//MAC verification
free(server_message->message);
server_message->message = dec_message;
handshake = RecordToHandshake(server_message);
handshake->length = dec_message_len;
handshake->length = handshake->length - ciphersuite_choosen->hash_size;
handshake->msg_type = FINISHED;
server_write_MAC_secret = key_block + ciphersuite_choosen->hash_size;
mac = dec_message + (dec_message_len - ciphersuite_choosen->hash_size);
mac_test = MAC(ciphersuite_choosen, handshake, server_write_MAC_secret);
if(ByteCompare(mac, mac_test, ciphersuite_choosen->hash_size)==0){
printf("\nmac verified\n");
}
else{
printf("\nmac not verified\n");
exit(1);
}
free(ciphersuite_choosen);
FreeRecordLayer(server_message);
FreeHandshake(handshake);
free(master_secret);
free(key_block);
free(mac_test);
return 0;
}
