diff --git a/src/tpm.c b/src/tpm.c index 34f4a1f..8ddca5d 100644 --- a/src/tpm.c +++ b/src/tpm.c @@ -441,11 +441,13 @@ cleanup: return rv; } -static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neighbour, uint8_t* src_addr) { +static int rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neighbour, uint8_t* src_addr) { + + int rv = 0; if (!tpm_rx->tpm->tollingType) { log_error("[tolling] received TPM does not have a type"); - return; + return 1; } TollingType_t* type_rx = tpm_rx->tpm->tollingType; @@ -464,7 +466,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig case TollingType_PR_entry: if (type_rx->choice.entry.present != TollingEntry_PR_request) { log_error("[tolling] received TPM.entry is not request"); - return; + return 1; } client_id = type_rx->choice.entry.choice.request.clientId; nonce = type_rx->choice.entry.choice.request.transactionNonce; @@ -477,7 +479,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig !type_rx->choice.exit->choice.request ) { log_error("[tolling] received TPM.exit is not request"); - return; + return 1; } if (tolling->protocol.p != TOLLING_PROTOCOL_GN_DPKI) { @@ -489,20 +491,21 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig if (!type_rx->choice.exit->choice.request->entryProof) { log_error("[tolling] received TPM.exit.request does not contain entry proof"); - return; + return 1; } TPM_t* ep = (TPM_t*) type_rx->choice.exit->choice.request->entryProof; if (!ep->tpmSignature) { log_error("[tolling] received TPM.exit.request.entryProof does not contain signature");; - return; + return 1; } // Encode TollingPaymentMessage asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, ep->tpm, buf1, buf_len); if (enc.encoded == -1) { log_error("[tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } @@ -538,16 +541,19 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf2, rl).code) { log_error("SecurityReply.verify decode failure"); + rv = 1; goto cleanup; } if (srep->returnCode == SecurityReplyReturnCode_rejected) { log_error("SecurityReply.verify rejected"); + rv = 1; goto cleanup; } if (srep->data->choice.verify.report != SecurityVerifyConfirmCode_success) { log_debug("entry proof signature verify failed"); + rv = 1; goto cleanup; } @@ -561,7 +567,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig case TollingType_PR_single: if (type_rx->choice.single.present != TollingSingle_PR_request) { log_error("[tolling] received TPM.single is not request"); - return; + return 1; } client_id = type_rx->choice.single.choice.request.clientId; nonce = type_rx->choice.single.choice.request.transactionNonce; @@ -570,7 +576,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig default: log_error("[tolling] received TPM has unrecognized type"); - return; + return 1; } @@ -607,6 +613,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig if (!tpm_rx->tpmSignature) { log_error("[tolling] in simple mode but TPM without signature received"); + rv = 1; goto cleanup; } @@ -614,6 +621,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, tpm_rx->tpm, buf1, buf_len); if (enc.encoded == -1) { log_error("[tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } @@ -649,16 +657,19 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf2, rl).code) { log_error("SecurityReply.verify decode failure"); + rv = 1; goto cleanup; } if (srep->returnCode == SecurityReplyReturnCode_rejected) { log_error("SecurityReply.verify rejected"); + rv = 1; goto cleanup; } if (srep->data->choice.verify.report != SecurityVerifyConfirmCode_success) { log_debug("signature verify failed"); + rv = 1; goto cleanup; } @@ -730,6 +741,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig if (!client) { if (tolling->station.rsu.clients_len-1 >= TOLLING_MAX_CLIENTS) { log_error("[tolling] max clients reached"); + rv = 1; goto cleanup; } tolling->station.rsu.clients[tolling->station.rsu.clients_len] = malloc(sizeof(toll_client_t)); @@ -787,6 +799,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, tpm->tpm, buf1, buf_len); if (enc.encoded == -1) { log_error("[tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } @@ -802,6 +815,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buf1+1, buf_len-1); if (enc.encoded == -1) { log_error("[tolling] error encoding SecurityRequest (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } log_debug("->[security] SecurityRequest.sign (%ldB)", enc.encoded+1); @@ -811,11 +825,13 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf2, rc).code) { log_error("SecurityReply.sign decode failure"); + rv = 1; goto cleanup; } if (srep->returnCode == SecurityReplyReturnCode_rejected) { log_error("SecurityReply.sign rejected"); + rv = 1; goto cleanup; } @@ -841,6 +857,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig enc = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm, tpm_uper, buf_len); if (enc.encoded == -1) { log_error("[tolling] error encoding TPM.reply (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } size_t tpm_uper_len = (enc.encoded + 7) / 8; @@ -917,6 +934,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig int rc = itss_0recv_rt(security_socket, buf2, buf_len, buf1, enc.encoded+1, 1000); if (rc == -1) { log_error("[tolling]-> SecurityRequest.tlsSend ->[security] "); + rv = 1; goto cleanup; } log_debug("[tolling]<- SecurityReply.tlsSend <-[security] | size:%dB", rc); @@ -928,6 +946,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig !srep->data || srep->data->present != SecurityReplyData_PR_tlsSend) { log_error("[tolling]<- SecurityReply.tlsSend rejected"); + rv = 1; goto cleanup; } @@ -964,6 +983,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf1+1, buf_len-1); if (enc.encoded == -1) { log_error("[tolling] error encoding TR TPM.reply (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } @@ -989,6 +1009,7 @@ static void rsu_handle_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neig enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buf1+1, buf_len-1); if (enc.encoded == -1) { log_error("[tolling] error encoding FI TPM.reply (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } @@ -1016,13 +1037,16 @@ cleanup: ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq); ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep); ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); + + return rv; } -static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_socket, itss_queue_t* tx_queue, uint8_t* neighbour, uint8_t* src_addr) { +static int veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_socket, itss_queue_t* tx_queue, uint8_t* neighbour, uint8_t* src_addr) { + int rv = 0; if (!tpm_rx->tpm->tollingType) { log_error("[tolling] received TPM does not have a type"); - return; + return 1; } TollingType_t* type_rx = tpm_rx->tpm->tollingType; @@ -1041,7 +1065,7 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s case TollingType_PR_entry: if (type_rx->choice.entry.present != TollingEntry_PR_reply) { log_error("[tolling] received TPM.entry is not reply"); - return; + return 1; } if (tolling->station.obu.toll_type != TollingType_PR_entry) { log_error("[tolling] received TPM toll (%d) is not expected toll type (%d)", @@ -1056,7 +1080,7 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s asn_enc_rval_t e_tep = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm_rx, b_tep, 2048); if (e_tep.encoded == -1) { log_error("[tolling] error encoding received TPM as entry proof"); - return; + return 1; } uper_decode_complete(NULL, &asn_DEF_TPM, (void**) &tolling->station.obu.entry_proof, b_tep, (e_tep.encoded+7)/8); break; @@ -1066,12 +1090,12 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s type_rx->choice.exit->present != TollingExit_PR_reply ) { log_error("[tolling] received TPM.exit is not reply"); - return; + return 1; } if (tolling->station.obu.toll_type != TollingType_PR_exit) { log_error("[tolling] received TPM toll (%d) is not expected toll type (%d)", TollingType_PR_exit, tolling->station.obu.toll_type); - return; + return 1; } client_id = type_rx->choice.exit->choice.reply.clientId; nonce = type_rx->choice.exit->choice.reply.transactionNonce; @@ -1082,7 +1106,7 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s case TollingType_PR_single: if (type_rx->choice.single.present != TollingSingle_PR_reply) { log_error("[tolling] received TPM.single is not reply"); - return; + return 1; } if (tolling->station.obu.toll_type != TollingType_PR_single) { log_error("[tolling] received TPM type (%d) is not expected toll type (%d)", @@ -1096,17 +1120,17 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s default: log_error("[tolling] received TPM has unrecognized type"); - return; + return 1; } if (client_id != tolling->station.obu.client_id) { log_debug("[tolling]<- received TPM.reply clientId different from ego"); - return; + return 1; } if (nonce != tolling->station.obu.nonce) { log_error("[tolling]<- received TPM.reply nonce different from sent request"); - return; + return 1; } @@ -1114,6 +1138,7 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, tpm_rx->tpm, buf1, buf_len); if (enc.encoded == -1) { log_error("[tolling] error encoding TollingPaymentMessage (%s)", enc.failed_type->name); + rv = 1; goto cleanup; } @@ -1150,16 +1175,19 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buf2, rl).code) { log_error("SecurityReply.verify decode failure"); + rv = 1; goto cleanup; } if (srep->returnCode == SecurityReplyReturnCode_rejected) { log_error("SecurityReply.verify rejected"); + rv = 1; goto cleanup; } if (srep->data->choice.verify.report != SecurityVerifyConfirmCode_success) { log_debug("signature verify failed"); + rv = 1; goto cleanup; } @@ -1241,6 +1269,8 @@ static void veh_handle_recv(tolling_t* tolling, TPM_t* tpm_rx, void** security_s cleanup: ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq); ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep); + + return rv; } int tpm_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neighbour, uint8_t* src_addr) { @@ -1280,11 +1310,12 @@ int tpm_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neighbour, uint8_t* log_debug("[tolling] received TPM.entry.reply, ignoring"); goto cleanup; } - itss_time_lock(); - log_info("[tolling] entry.reply took %lld us", itss_ts_get(TIME_MICROSECONDS) - tolling->station.obu.t_init); - itss_time_unlock(); - tolling->station.obu.rt_on = false; - veh_handle_recv(tolling, tpm_rx, security_socket, facilities.tx_queue, neighbour, src_addr); + if (!veh_handle_recv(tolling, tpm_rx, security_socket, facilities.tx_queue, neighbour, src_addr)) { + itss_time_lock(); + log_info("[tolling] entry.reply took %lld us", itss_ts_get(TIME_MICROSECONDS) - tolling->station.obu.t_init); + itss_time_unlock(); + tolling->station.obu.rt_on = false; + } break; default: break; @@ -1308,11 +1339,12 @@ int tpm_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neighbour, uint8_t* log_debug("[tolling] received TPM.exit.reply, ignoring"); goto cleanup; } - itss_time_lock(); - log_info("[tolling] exit.reply took %lld us", itss_ts_get(TIME_MICROSECONDS) - tolling->station.obu.t_init); - itss_time_unlock(); - tolling->station.obu.rt_on = false; - veh_handle_recv(tolling, tpm_rx, security_socket, facilities.tx_queue, neighbour, src_addr); + if (!veh_handle_recv(tolling, tpm_rx, security_socket, facilities.tx_queue, neighbour, src_addr)) { + itss_time_lock(); + log_info("[tolling] exit.reply took %lld us", itss_ts_get(TIME_MICROSECONDS) - tolling->station.obu.t_init); + itss_time_unlock(); + tolling->station.obu.rt_on = false; + } break; default: break; @@ -1333,11 +1365,12 @@ int tpm_recv(TPM_t* tpm_rx, void** security_socket, uint8_t* neighbour, uint8_t* log_debug("[tolling] received TPM.single.reply, ignoring"); goto cleanup; } - itss_time_lock(); - log_info("[tolling] single.reply took %lld us", itss_ts_get(TIME_MICROSECONDS) - tolling->station.obu.t_init); - itss_time_unlock(); - tolling->station.obu.rt_on = false; - veh_handle_recv(tolling, tpm_rx, security_socket, facilities.tx_queue, neighbour, src_addr); + if (!veh_handle_recv(tolling, tpm_rx, security_socket, facilities.tx_queue, neighbour, src_addr)) { + itss_time_lock(); + log_info("[tolling] single.reply took %lld us", itss_ts_get(TIME_MICROSECONDS) - tolling->station.obu.t_init); + itss_time_unlock(); + tolling->station.obu.rt_on = false; + } break; default: break;