u8 aes_128_cbc_decrypt(u32* key, u32* iv, u8 *data, u16 data_len)
{
u32 cbc[AES_BLOCK_SIZE], tmp[AES_BLOCK_SIZE];
u8 *pos = data;
u32 data_tmp[AES_BLOCK_SIZE];
u16 data_size = 0;
int i, j, blocks;
// Decryption key scheduling, to be done once
AES_keyschedule_dec((u32*)key,(u32*)exp_key);
data_size = AES_BLOCK_SIZE*4;
memcpy(cbc, iv, data_size);
blocks = (data_len/data_size);
for (i = 0; i < blocks; i++)
{
memcpy(data_tmp, pos, data_size);
for(j = 0;j<AES_BLOCK_SIZE;j++)
tmp[j] = littleToBig(data_tmp[j]);
AES_decrypt(tmp, data_tmp, exp_key);
for (j = 0; j < AES_BLOCK_SIZE; j++)
data_tmp[j] ^= cbc[j];
memcpy(cbc, tmp, data_size);
for(j = 0;j<AES_BLOCK_SIZE;j++)
tmp[j] = bigToLittle(data_tmp[j]);
memcpy(pos, tmp, data_size);
pos += data_size;
}
return 0;
}
/*******************************************************************************
* Function Name : WMBus_AppliDecrypt
* Description : Does key expension before decryption starts
* and put the data stream back into payload buffer
* Input : - input_pointer: input payload address
* - input pointer: encryption key
* - int: length
* Output : None
* Return : None
*******************************************************************************/
void WMBus_AppliDecrypt(u8 *key, unsigned char *payload, int length)
{
/*******************************************************/
/* AES CTR DECRYPTION */
/*******************************************************/
/* Decryption key scheduling, to be done once */
AES_keyschedule_dec((u8*)key,(u32*)exp_key);
/* decryption */
decrypt_pdu_cbc((u32*)exp_key,payload,length,payload);
}
ot_int network_route_ff(m2session* session) {
ot_int route_val;
/// Strip CRC (-2 bytes)
rxq.front[0] -= 2;
/// Acquire Flags and Protocol from the Frame Info Field
rxq.getcursor = &rxq.front[3];
session->protocol = (*rxq.getcursor & M2FI_FRTYPEMASK);
session->flags = *rxq.getcursor & 0xC0;
m2np.header.fr_info = *rxq.getcursor++;
/// Treat Non-Mode-2 Protocols
/// @note Non-Mode-2 protocols not supported at this time
if (m2np.header.fr_info & M2FI_NM2) {
return -1;
}
/// Data Link Layer Security
if (m2np.header.fr_info & M2FI_DLLS) {
# if (OT_FEATURE(DLL_SECURITY))
///@todo experimental
AES_load_static_key(ISF_ID(user_authentication_key), (ot_u32*)txq.front);
AES_keyschedule_dec((ot_u32*)txq.front, (ot_u32*)(txq.front+16));
AES_decrypt(rxq.getcursor, rxq.getcursor, (ot_u32*)(txq.front+16));
# else
return -1;
# endif
}
/// Address Control Header (Present in M2NP)
/// Session Connection and Dialog Filtering:
/// - if unassociated, connect now
/// - if already connected, make sure the dialog IDs are equal
if (m2np.header.fr_info & M2FI_ENADDR) {
if (session->netstate & M2_NETSTATE_CONNECTED) {
if (session->dialog_id != q_readbyte(&rxq)) {
return -1;
}
}
else {
session->netstate |= M2_NETSTATE_CONNECTED;
session->subnet = rxq.front[2];
session->dialog_id = q_readbyte(&rxq);
}
/// Grab global flags from Address Control
m2np.header.addr_ctl = q_readbyte(&rxq);
session->flags |= m2np.header.addr_ctl & 0x3F;
/// Grab Source Address from this packet (dialog address), which is
/// converted to the target address in the response.
m2np.rt.dlog.length = (m2np.header.addr_ctl & M2_FLAG_VID) ? 2 : 8;
m2np.rt.dlog.value = q_markbyte(&rxq, m2np.rt.dlog.length);
/// Network Layer Security
/// @note Network Layer Security not supported at this time
if (m2np.header.addr_ctl & M2_FLAG_NLS) {
# if (OT_FEATURE(NL_SECURITY))
# else
return -1;
# endif
}
/// If unicasting, the next data is the target address, which will have
/// the same length as the source address, and it needs to match this
/// device's device ID (VID or UID)
if ((m2np.header.addr_ctl & 0xC0) == 0) {
session->netstate |= M2_NETFLAG_FIRSTRX;
if ( !m2np_idcmp(m2np.rt.dlog.length, q_markbyte(&rxq, m2np.rt.dlog.length)) ) {
return -1;
}
}
}
/// Vector to the appropriate Network Layer Protocol Parser
/// Most network protocols don't do anything except broadcast. M2NP is the
/// exception, and it manages various types of routing at the network layer.
route_val = -1;
switch (session->protocol & M2FI_FRTYPEMASK) {
case M2FI_FRDIALOG:
case M2FI_FRNACK: {
// Reset routing template
m2np.rt.hop_code = 0;
m2np.rt.hop_ext = 0;
m2np.rt.orig.value = NULL;
m2np.rt.dest.value = NULL;
/// Unicast and Anycast Requests have a routing template
/// (Not currently supported, so just move the cursor ahead)
if ((m2np.header.addr_ctl & 0x40) == 0) {
m2np.rt.hop_code = q_readbyte(&rxq);
m2np.rt.orig.length = ((m2np.rt.hop_code & M2HC_VID) != 0) ? 2 : 8;
m2np.rt.dest.length = m2np.rt.orig.length;
if ((m2np.rt.hop_code & M2HC_EXT) != 0) {
m2np.rt.hop_ext = q_readbyte(&rxq);
}
if ((m2np.rt.hop_code & M2HC_ORIG) != 0) {
m2np.rt.orig.value = q_markbyte(&rxq, m2np.rt.orig.length);
}
if ((m2np.rt.hop_code & M2HC_DEST) != 0) {
m2np.rt.dest.value = q_markbyte(&rxq, m2np.rt.dest.length);
}
}
} // Note case fall-through
case M2FI_STREAM: {
/// M2DP gets parsed just like M2NP, but it uses the Network data
/// stored from the last M2NP frame
route_val = m2qp_parse_frame(session);
break;
}
case M2FI_RFU: break;
}
/// Attach footer to response, if necessary
if (route_val >= 0) {
m2np_footer(session);
}
# if (OT_FEATURE(M2NP_CALLBACKS) == ENABLED) && \
!defined(EXTF_network_sig_route)
m2np.signal.route(route_val, session->protocol);
# elif defined(EXTF_network_sig_route)
network_sig_route(route_val, session->protocol);
# endif
return route_val;
}
