int pack_item(tlv_t * item, void * outbuf, uint32_t * outsize)
{
int ret = 0;
if ((NULL == item) || (NULL == outbuf) || (NULL == outsize)) {
return -1;
}
switch (item->type) {
case TLV_TYPE_INT8:
case TLV_TYPE_UINT8:
ret = pack_int8((uint8_t *)(item->value), outbuf, outsize);
break;
case TLV_TYPE_INT16:
case TLV_TYPE_UINT16:
ret = pack_int16((uint16_t *)(item->value), outbuf, outsize);
break;
case TLV_TYPE_INT32:
case TLV_TYPE_UINT32:
ret = pack_int32((uint32_t *)(item->value), outbuf, outsize);
break;
case TLV_TYPE_BYTES:
ret = pack_bytes(item->value, item->length, outbuf, outsize);
break;
case TLV_TYPE_MSG:
ret = msg_pack((message_t *)(item->value), outbuf, outsize);
break;
default:
ret = -1;
break;
}
return ret;
}
Octstr *wtls_payload_pack(wtls_Payload *payload) {
Octstr *data;
long bitpos, charpos;
long messageSizePos, sizepos;
/* Used for length calculations */
int size;
/* We rely on octstr_set_bits to lengthen our octstr as needed. */
data = octstr_create("");
bitpos = 0;
charpos = 0;
sizepos = 0;
/* the record field length flag - always present*/
octstr_set_bits(data, bitpos, 1, 1);
bitpos += 1;
/* the sequence number flag */
octstr_set_bits(data, bitpos, 1, payload->seqnum);
bitpos += 1;
/* the cipher usage flag */
octstr_set_bits(data, bitpos, 1, payload->cipher);
bitpos += 1;
/* the reserved bit */
octstr_set_bits(data, bitpos, 1, payload->reserved);
bitpos += 1;
/* set the message type */
octstr_set_bits(data, bitpos, 4, payload->type);
bitpos += 4;
charpos += 1;
/* set the sequence number if present */
if(payload->seqnum) {
charpos = pack_int16(data, charpos, payload->seqnum);
}
/* set the WTLS length */
charpos = pack_int16(data, charpos, payload->rlen);
/* append the data from the wtls_PDU */
octstr_insert(data, payload->data, octstr_len(data));
return data;
}
int stlink2_write_word(programmer_t *pgm, unsigned int word, unsigned int start) {
unsigned char buf[4], start2[2];
pack_int16(start, start2);
stlink2_cmd(pgm, 0xf40a, 8,
0x00, 0x02,
0x00, 0x00,
HI(start), LO(start),
HI(word), LO(word));
usleep(2000);
return(stlink2_get_status(pgm)); // Should be '1'
}
int stlink2_write_byte(programmer_t *pgm, unsigned char byte, unsigned int start) {
unsigned char buf[4], start2[2];
pack_int16(start, start2);
stlink2_cmd(pgm, 0xf40a, 7,
0x00, 0x01,
0x00, 0x00,
HI(start), LO(start),
byte);
usleep(2000);
return(stlink2_get_status(pgm)); // Should be '1'
}
int stlink2_write_and_read_byte(programmer_t *pgm, unsigned char byte, unsigned int start) {
unsigned char buf[4], start2[2];
pack_int16(start, start2);
stlink2_cmd(pgm, 0xf40b, 7,
0x00, 0x01,
0x00, 0x00,
HI(start), LO(start),
byte);
usleep(2000);
stlink2_get_status(pgm);
stlink2_cmd(pgm, 0xf40c, 0);
return(msg_recv_int8(pgm));
}
int
pack_value (tlv_t *tlv, void *outbuf, uint32_t *out_sz) {
if (NULL == tlv || NULL == outbuf || NULL == out_sz) {
return -1;
}
int b = 0;
switch (tlv->type) {
case TLV_TYPE_INT8:
case TLV_TYPE_UINT8:
b = pack_int8((uint8_t*)(tlv->value), outbuf, out_sz);
break;
case TLV_TYPE_INT16:
case TLV_TYPE_UINT16:
b = pack_int16((uint16_t*)(tlv->value), outbuf, out_sz);
break;
case TLV_TYPE_INT32:
case TLV_TYPE_UINT32:
b = pack_int32((uint32_t*)(tlv->value), outbuf, out_sz);
break;
case TLV_TYPE_BYTES:
b = pack_bytes(tlv->value, tlv->length, outbuf, out_sz);
break;
default:
b = -1;
break;
}
return b;
}
/* This function will convert our buffer into a completed GenericBlockCipher */
Octstr *wtls_encrypt(Octstr * buffer, WTLSMachine * wtls_machine,
int recordType)
{
Octstr *bufferCopy;
Octstr *encryptedContent;
Octstr *contentMac;
Octstr *tempData;
char *tempPadding = NULL;
int paddingLength, macSize, blockLength, bufferLength, refresh;
int i;
refresh = 1 << wtls_machine->key_refresh;
if (!(wtls_machine->server_seq_num % refresh))
calculate_server_key_block(wtls_machine);
/* Copy our buffer */
bufferCopy = octstr_duplicate(buffer);
/* Get the MAC of the content */
bufferLength = octstr_len(buffer);
/* Copy the buffer in preparation for MAC calculation */
tempData = octstr_create("");
pack_int16(tempData, 0, wtls_machine->server_seq_num);
octstr_append_char(tempData, recordType);
pack_int16(tempData, octstr_len(tempData), bufferLength);
octstr_append(tempData, buffer);
/* Calculate the MAC */
contentMac =
wtls_hmac_hash(wtls_machine->server_write_MAC_secret, tempData,
wtls_machine->mac_algorithm);
/* Calculate the padding length */
macSize = hash_table[wtls_machine->mac_algorithm].mac_size;
blockLength =
bulk_table[wtls_machine->bulk_cipher_algorithm].block_size;
paddingLength =
blockLength - ((bufferLength + macSize + 1) % blockLength);
/* Append the MAC to the bufferCopy */
octstr_append(bufferCopy, contentMac);
if (paddingLength > 0) {
/* Pad with the paddingLength itself paddingLength times. Confused yet? */
tempPadding = gw_malloc(paddingLength);
for (i = 0; i < paddingLength; i++) {
/* You're probably really spaced out around now...
see section 9.2.3.3 for more details... */
tempPadding[i] = paddingLength;
}
octstr_append_data(bufferCopy, tempPadding, paddingLength);
gw_free(tempPadding);
}
/* Add the length byte */
octstr_append_char(bufferCopy, paddingLength);
/* Encrypt the content */
switch (wtls_machine->bulk_cipher_algorithm) {
case NULL_bulk:
encryptedContent = octstr_duplicate(bufferCopy);
break;
case RC5_CBC:
case RC5_CBC_40:
case RC5_CBC_56:
encryptedContent =
wtls_rc5(bufferCopy, wtls_machine, RC5_ENCRYPT);
break;
case DES_CBC:
case DES_CBC_40:
encryptedContent =
wtls_des(bufferCopy, wtls_machine, DES_ENCRYPT);
break;
default:
error(0,
"wtls_encrypt: Unsupported bulk cipher algorithm (%d).",
wtls_machine->bulk_cipher_algorithm);
encryptedContent = NULL;
break;
}
octstr_destroy(bufferCopy);
octstr_destroy(contentMac);
octstr_destroy(tempData);
octstr_destroy(buffer);
return (encryptedContent);
}
Octstr *wtls_decrypt(wtls_Payload * payload, WTLSMachine * wtls_machine)
{
int len, padLen = 0, macSize, recordType, block, refresh;
Octstr *openText, *MAContent, *tempData, *result;
char cipher[20], *p;
if (payload->seqNum && wtls_machine->client_seq_num > payload->seqNum) {
error(0,
"Out of sequence packet received (p: %d < %d :w). Dropping datagram.",
payload->seqNum, wtls_machine->client_seq_num);
return (NULL);
} else
wtls_machine->client_seq_num = payload->seqNum;
refresh = 1 << wtls_machine->key_refresh;
if (wtls_machine->last_refresh < 0 || (wtls_machine->last_refresh +
refresh <=
wtls_machine->client_seq_num))
calculate_client_key_block(wtls_machine);
switch (wtls_machine->bulk_cipher_algorithm) {
case NULL_bulk:
openText = octstr_duplicate(payload->data);
break;
case RC5_CBC:
case RC5_CBC_40:
case RC5_CBC_56:
openText = wtls_rc5(payload->data, wtls_machine, RC5_DECRYPT);
break;
case DES_CBC:
case DES_CBC_40:
openText = wtls_des(payload->data, wtls_machine, DES_DECRYPT);
break;
default:
cipherName(cipher, wtls_machine->bulk_cipher_algorithm);
error(0,
"wtls_decrypt: Unsupported bulk cipher algorithm (%s).",
cipher);
return (NULL);
break;
}
/* Verify MAC */
recordType = 1 << 7;
recordType |= payload->snMode << 6;
recordType |= payload->cipher << 5;
recordType |= payload->reserved << 4;
recordType |= payload->type;
len = octstr_len(openText);
p = octstr_get_cstr(openText);
block = bulk_table[wtls_machine->bulk_cipher_algorithm].block_size;
padLen = *(p + len - 1);
if (padLen >= block || padLen != *(p + len - 2))
padLen = 0;
padLen++;
macSize = hash_table[wtls_machine->mac_algorithm].mac_size;
tempData = octstr_create("");
pack_int16(tempData, 0, wtls_machine->client_seq_num);
octstr_append_char(tempData, recordType);
pack_int16(tempData, 3, len - macSize - padLen);
octstr_append_data(tempData, p, len - macSize - padLen);
MAContent = wtls_hmac_hash(wtls_machine->client_write_MAC_secret,
tempData, wtls_machine->mac_algorithm);
if (memcmp(octstr_get_cstr(MAContent), p + len - padLen - macSize,
macSize)) {
octstr_destroy(MAContent);
octstr_destroy(tempData);
octstr_destroy(openText);
error(0, "wtls_decrypt: Rejected packet due to bad MAC");
return (NULL);
}
octstr_destroy(MAContent);
octstr_destroy(tempData);
result = octstr_create_from_data((char *)p, len - padLen - macSize);
octstr_destroy(openText);
return (result);
}
wtls_Payload *wtls_pdu_pack(wtls_PDU *pdu, WTLSMachine* wtls_machine) {
Octstr *data, *buffer, *encryptedbuffer;
wtls_Payload *payload;
long bitpos, charpos;
long messageSizePos, sizepos;
/* Used for length calculations */
int size, recordType;
/* create the wtls_PDU */
payload = (wtls_Payload *)gw_malloc(sizeof(wtls_Payload));
payload->type = pdu->type;
payload->reserved = pdu->reserved;
payload->cipher = pdu->cipher;
payload->seqnum = pdu->seqnum;
/* We rely on octstr_set_bits to lengthen our octstr as needed. */
data = octstr_create("");
buffer = octstr_create("");
bitpos = 0;
charpos = 0;
sizepos = 0;
switch (pdu->type) {
case ChangeCipher_PDU:
octstr_append_char(buffer, pdu->u.cc.change);
charpos += 1;
break;
case Alert_PDU:
octstr_append_char(buffer, pdu->u.alert.level);
charpos += 1;
octstr_append_char(buffer, pdu->u.alert.desc);
charpos += 1;
charpos = pack_octstr_fixed(buffer, charpos, pdu->u.alert.chksum);
charpos += 1;
break;
case Handshake_PDU:
octstr_append_char(buffer, pdu->u.handshake.msg_type);
charpos += 1;
/* Save the location of the message size */
messageSizePos = charpos;
charpos = pack_int16 (buffer, charpos, pdu->u.handshake.length);
switch (pdu->u.handshake.msg_type) {
case hello_request:
break;
case client_hello:
octstr_append_char(buffer, pdu->u.handshake.client_hello->clientversion);
charpos += 1;
charpos = pack_random(buffer, charpos, pdu->u.handshake.client_hello->random);
octstr_append_char(buffer, octstr_len(
pdu->u.handshake.client_hello->session_id));
charpos += 1;
charpos = pack_octstr(buffer, charpos, pdu->u.handshake.client_hello->session_id);
/* pack the list of keys */
charpos = pack_key_list(buffer, charpos,
pdu->u.handshake.client_hello->client_key_ids);
charpos = pack_key_list(buffer, charpos,
pdu->u.handshake.client_hello->trusted_key_ids);
/* pack the list of CipherSuites */
charpos = pack_ciphersuite_list(buffer, charpos,
pdu->u.handshake.client_hello->ciphersuites);
/* CompressionMethods */
charpos = pack_compression_method_list(buffer, charpos,
pdu->u.handshake.client_hello->comp_methods);
octstr_append_char(buffer, pdu->u.handshake.client_hello->snmode);
charpos += 1;
octstr_append_char(buffer, pdu->u.handshake.client_hello->krefresh);
charpos += 1;
break;
case server_hello:
octstr_append_char(buffer, pdu->u.handshake.server_hello->serverversion);
charpos += 1;
charpos = pack_random(buffer, charpos, pdu->u.handshake.server_hello->random);
charpos = pack_octstr(buffer, charpos, pdu->u.handshake.server_hello->session_id);
charpos += 1;
octstr_append_char(buffer, pdu->u.handshake.server_hello->
client_key_id);
charpos += 1;
/* CypherSuite */
octstr_append_char(buffer, pdu->u.handshake.server_hello->
ciphersuite->bulk_cipher_algo);
charpos += 1;
octstr_append_char(buffer, pdu->u.handshake.server_hello->
ciphersuite->mac_algo);
charpos += 1;
/* CompressionMethod */
octstr_append_char(buffer, pdu->u.handshake.server_hello->comp_method);
charpos += 1;
octstr_append_char(buffer, pdu->u.handshake.server_hello->snmode);
charpos += 1;
octstr_append_char(buffer, pdu->u.handshake.server_hello->krefresh);
charpos += 1;
break;
case certificate:
octstr_append_char(buffer, pdu->u.handshake.certificate->certificateformat);
charpos += 1;
switch (pdu->u.handshake.certificate->certificateformat) {
case WTLSCert:
charpos = pack_wtls_certificate(buffer, charpos, pdu->u.handshake.certificate->wtls_certificate);
break;
case X509Cert:
charpos = pack_octstr16(buffer, charpos, pdu->u.handshake.certificate->x509_certificate);
break;
case X968Cert:
charpos = pack_octstr16(buffer, charpos, pdu->u.handshake.certificate->x968_certificate);
break;
}
break;
case server_key_exchange:
debug("wtls: ", 0,"Packing ServerKeyExchange");
/* pack the ParameterSpecifier */
charpos = pack_param_spec(buffer, charpos, pdu->u.handshake.server_key_exchange->param_spec);
switch (client_key_exchange_algo) {
case rsa_anon:
charpos = pack_rsa_pubkey(buffer, charpos, pdu->u.handshake.server_key_exchange->rsa_params);
break;
case dh_anon:
charpos = pack_dh_pubkey(buffer, charpos, pdu->u.handshake.server_key_exchange->dh_params);
break;
case ecdh_anon:
charpos = pack_ec_pubkey(buffer, charpos, pdu->u.handshake.server_key_exchange->ecdh_params);
break;
}
break;
case client_key_exchange:
switch (client_key_exchange_algo) {
case rsa:
case rsa_anon:
charpos = pack_rsa_encrypted_secret(buffer, charpos, pdu->u.handshake.client_key_exchange->rsa_params);
break;
case dh_anon:
charpos = pack_dh_pubkey(buffer, charpos, pdu->u.handshake.client_key_exchange->dh_anon_params);
break;
case ecdh_anon:
case ecdh_ecdsa:
charpos = pack_ec_pubkey(buffer, charpos, pdu->u.handshake.client_key_exchange->ecdh_params);
break;
}
break;
case server_hello_done:
/* empty */
break;
case finished:
charpos = pack_octstr_fixed(buffer, charpos, pdu->u.handshake.finished->verify_data);
debug("wtls", 0, "verify_data (in pack)");
octstr_dump(pdu->u.handshake.finished->verify_data,0 );
break;
}
/* Change the length */
size = octstr_len(buffer) - messageSizePos - 2;
debug("wtls_msg.c:length",0,"Setting msg size to : %d",size);
octstr_set_char(buffer, messageSizePos, (size & 0xFF00) >> 8);
messageSizePos += 1;
octstr_set_char(buffer, messageSizePos, (size & 0x00FF));
/* we keep the handshake data to create the Finished PDU */
octstr_append(wtls_machine->handshake_data, buffer);
break;
case Application_PDU:
/* application message */
charpos += pack_octstr(data, charpos, pdu->u.application.data);
break;
default:
panic(0, "Packing unknown WTLS PDU type %ld", (long) pdu->type);
}
/* encrypt the buffer if needed */
if(pdu->cipher) {
/* the MAC is calculated with the record type so we need it now */
recordType = 1 << 7; /* length, always present */
recordType |= pdu->seqnum << 6;
recordType |= pdu->cipher << 5;
recordType |= pdu->reserved << 4;
recordType |= pdu->type;
encryptedbuffer = wtls_encrypt(buffer, wtls_machine, recordType);
payload->data = encryptedbuffer;
}
else {
payload->data = buffer;
}
payload->rlen = octstr_len(payload->data);
debug("wtls", 0, "Packed PDU Length: %d", payload->rlen);
return payload;
}
static void exchange_keys(WAPEvent * event, WTLSMachine * wtls_machine)
{
RSAPublicKey *public_key = NULL;
Octstr *checking_data = NULL;
/* The Wap PDUs we have to dispatch */
wtls_PDU *changeCipherSpecPDU;
wtls_PDU *finishedPDU;
/* The PDUs we have to process */
wtls_Payload *tempPayload;
wtls_PDU *clientKeyXchgPDU;
wtls_PDU *changeCipherSpec_incoming_PDU;
wtls_PDU *finished_incoming_PDU;
/* For decrypting/encrypting data */
Octstr *concatenatedRandoms = NULL;
Octstr *encryptedData = NULL;
Octstr *decryptedData = NULL;
Octstr *labelVerify = NULL;
Octstr *labelMaster = NULL;
/* Process the incoming event : ClientKeyExchange */
tempPayload =
(wtls_Payload *) gwlist_search(event->u.T_Unitdata_Ind.pdu_list,
(void *)client_key_exchange,
match_handshake_type);
if (!tempPayload) {
error(0, "Missing client_key_exchange. Aborting...");
fatalAlert(event, unexpected_message);
return;
}
/* Keep the data so we can send it back */
octstr_insert(wtls_machine->handshake_data, tempPayload->data,
octstr_len(wtls_machine->handshake_data));
clientKeyXchgPDU = wtls_pdu_unpack(tempPayload, wtls_machine);
wtls_pdu_dump(clientKeyXchgPDU, 0);
/* Decrypt the client key exchange PDU */
encryptedData =
clientKeyXchgPDU->u.handshake.client_key_exchange->rsa_params->
encrypted_secret;
decryptedData =
wtls_decrypt_key(wtls_machine->key_algorithm, encryptedData);
if (!decryptedData) {
error(0,
"Key Exchange failed. Couldn't decrypt client's secret (%d)."
" Aborting...", wtls_machine->key_algorithm);
wtls_pdu_destroy(clientKeyXchgPDU);
fatalAlert(event, decryption_failed);
return;
}
public_key = wtls_get_rsapublickey();
pack_int16(decryptedData, octstr_len(decryptedData),
octstr_len(public_key->rsa_exponent));
octstr_insert(decryptedData, public_key->rsa_exponent,
octstr_len(decryptedData));
pack_int16(decryptedData, octstr_len(decryptedData),
octstr_len(public_key->rsa_modulus));
octstr_insert(decryptedData, public_key->rsa_modulus,
octstr_len(decryptedData));
/* Concatenate our random data */
concatenatedRandoms = octstr_cat(wtls_machine->client_random,
wtls_machine->server_random);
/* Generate our master secret */
labelMaster = octstr_create("master secret");
wtls_machine->master_secret = wtls_calculate_prf(decryptedData,
labelMaster,
concatenatedRandoms,
20, wtls_machine);
/* Process the incoming event : ChangeCipherSpec */
tempPayload =
(wtls_Payload *) gwlist_search(event->u.T_Unitdata_Ind.pdu_list,
(void *)ChangeCipher_PDU,
match_pdu_type);
if (!tempPayload) {
error(0, "Missing change_cipher. Aborting...");
octstr_destroy(labelMaster);
octstr_destroy(concatenatedRandoms);
destroy_rsa_pubkey(public_key);
octstr_destroy(decryptedData);
octstr_destroy(encryptedData);
fatalAlert(event, unexpected_message);
return;
}
changeCipherSpec_incoming_PDU = wtls_pdu_unpack(tempPayload,
wtls_machine);
octstr_dump(wtls_machine->client_write_MAC_secret, 0);
wtls_pdu_dump(changeCipherSpec_incoming_PDU, 0);
if (changeCipherSpec_incoming_PDU->u.cc.change == 1) {
debug("wtls", 0, "Need to decrypt the PDUs from now on...");
wtls_decrypt_pdu_list(wtls_machine,
event->u.T_Unitdata_Ind.pdu_list);
}
/* Process the incoming event : Finished */
tempPayload =
(wtls_Payload *) gwlist_search(event->u.T_Unitdata_Ind.pdu_list,
(void *)finished,
match_handshake_type);
if (!tempPayload) {
error(0, "Failed to decrypt finished PDU. Aborting...");
wtls_pdu_destroy(changeCipherSpec_incoming_PDU);
octstr_destroy(labelMaster);
octstr_destroy(concatenatedRandoms);
destroy_rsa_pubkey(public_key);
octstr_destroy(decryptedData);
octstr_destroy(encryptedData);
fatalAlert(event, decrypt_error);
return;
}
finished_incoming_PDU = wtls_pdu_unpack(tempPayload, wtls_machine);
debug("wtls", 0, "Client Finished PDU:");
wtls_pdu_dump(finished_incoming_PDU, 0);
/* Check the verify_data */
labelVerify = octstr_create("client finished");
checking_data = wtls_calculate_prf(wtls_machine->master_secret,
labelVerify,
(Octstr *) wtls_hash(wtls_machine->
handshake_data,
wtls_machine),
12, wtls_machine);
if (octstr_compare
(finished_incoming_PDU->u.handshake.finished->verify_data,
checking_data) == 0) {
wtls_machine->encrypted = 1;
debug("wtls", 0, "DATA VERIFICATION OK");
}
/* Keep the data so we can send it back in the next message
* octstr_insert(wtls_machine->handshake_data, tempPayload->data,
* octstr_len(wtls_machine->handshake_data));
*/
// temporary fix
octstr_truncate(tempPayload->data, 15);
octstr_insert(wtls_machine->handshake_data, tempPayload->data,
octstr_len(wtls_machine->handshake_data));
/* Create a new PDU List containing a ChangeCipherSpec and a Finished */
changeCipherSpecPDU = wtls_pdu_create(ChangeCipher_PDU);
changeCipherSpecPDU->u.cc.change = 1;
changeCipherSpecPDU->rlen = 1;
changeCipherSpecPDU->snMode =
wtls_machine->sequence_number_mode ? 1 : 0;
/* Generate our verify data */
finishedPDU = wtls_pdu_create(Handshake_PDU);
finishedPDU->u.handshake.msg_type = finished;
finishedPDU->cipher = 1;
finishedPDU->rlen = 1;
finishedPDU->snMode = wtls_machine->sequence_number_mode ? 1 : 0;;
finishedPDU->u.handshake.finished = gw_malloc(sizeof(Finished));
octstr_destroy(labelVerify);
labelVerify = octstr_create("server finished");
finishedPDU->u.handshake.finished->verify_data = wtls_calculate_prf
(wtls_machine->master_secret, labelVerify, (Octstr *) wtls_hash
(wtls_machine->handshake_data, wtls_machine), 12, wtls_machine);
/* Reset the accumulated Handshake data */
octstr_destroy(wtls_machine->handshake_data);
wtls_machine->handshake_data = octstr_create("");
/* Add the pdus to our list */
add_pdu(wtls_machine, changeCipherSpecPDU);
add_pdu(wtls_machine, finishedPDU);
/* Send it off */
send_queuedpdus(wtls_machine);
octstr_destroy(labelMaster);
octstr_destroy(labelVerify);
octstr_destroy(decryptedData);
octstr_destroy(encryptedData);
octstr_destroy(concatenatedRandoms);
wtls_pdu_destroy(finished_incoming_PDU);
wtls_pdu_destroy(changeCipherSpec_incoming_PDU);
wtls_pdu_destroy(finishedPDU);
wtls_pdu_destroy(changeCipherSpecPDU);
octstr_destroy(checking_data);
destroy_rsa_pubkey(public_key);
}
