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; }