ftcd_comm_base.cpp

00001 // ----------------------------------------------------------------------------
00002 // Copyright 2016-2017 ARM Ltd.
00003 //  
00004 // Licensed under the Apache License, Version 2.0 (the "License");
00005 // you may not use this file except in compliance with the License.
00006 // You may obtain a copy of the License at
00007 //  
00008 //     http://www.apache.org/licenses/LICENSE-2.0
00009 //  
00010 // Unless required by applicable law or agreed to in writing, software
00011 // distributed under the License is distributed on an "AS IS" BASIS,
00012 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013 // See the License for the specific language governing permissions and
00014 // limitations under the License.
00015 // ----------------------------------------------------------------------------
00016 
00017 #include <stdlib.h>
00018 #include <string.h>
00019 #include "pv_endian.h"
00020 #include "pv_log.h"
00021 #include "ftcd_comm_base.h"
00022 #include "cs_hash.h"
00023 #include "fcc_malloc.h"
00024 
00025 #define TRACE_GROUP "fcbs"
00026 
00027 FtcdCommBase::FtcdCommBase(ftcd_comm_network_endianness_e network_endianness, const uint8_t *header_token, bool use_signature)
00028 {
00029     _network_endianness = network_endianness;
00030     _header_token = NULL;
00031     _use_token = (header_token != NULL);
00032     if (_use_token) {
00033         _header_token = (uint8_t*)fcc_malloc(FTCD_MSG_HEADER_TOKEN_SIZE_BYTES);
00034         if (_header_token == NULL) {
00035             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed to allocate token buffer");
00036         } else {
00037             memcpy(_header_token, header_token, FTCD_MSG_HEADER_TOKEN_SIZE_BYTES);
00038         }
00039     }
00040 
00041     _use_signature = use_signature;
00042 }
00043 
00044 FtcdCommBase::~FtcdCommBase()
00045 {
00046     if (_header_token) {
00047         fcc_free(_header_token);
00048     }
00049 }
00050 
00051 
00052 bool FtcdCommBase::init()
00053 {
00054     return true;
00055 }
00056 
00057 void FtcdCommBase::finish()
00058 {
00059 }
00060 
00061 ftcd_comm_status_e FtcdCommBase::wait_for_message(uint8_t **message_out, uint32_t *message_size_out)
00062 {
00063     bool success = false;
00064     ftcd_comm_status_e status_code = FTCD_COMM_STATUS_SUCCESS;
00065     uint8_t *message = NULL;
00066     uint32_t message_size = 0;
00067 
00068     if (message_out == NULL || message_size_out == NULL) {
00069         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Invalid parameter");
00070         return FTCD_COMM_INVALID_PARAMETER;
00071     }
00072 
00073     *message_out = NULL;
00074     *message_size_out = 0;
00075 
00076     if (_use_token == true) {
00077         //detect token
00078         status_code = is_token_detected();
00079         if (status_code == FTCD_COMM_NETWORK_TIMEOUT) {
00080             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Network timeout occurred");
00081             return status_code;
00082         } else if (status_code == FTCD_COMM_NETWORK_CONNECTION_ERROR) {
00083             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Network connection error occurred");
00084             return status_code;
00085         }
00086     }
00087 
00088     // Read message size
00089     message_size = read_message_size();
00090     if (_network_endianness == FTCD_COMM_NET_ENDIANNESS_LITTLE) {
00091         message_size = pv_le32_to_h(message_size);
00092     } else { // big endian
00093         message_size = pv_be32_to_h(message_size);
00094     }
00095     if (message_size == 0) {
00096         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Unable to read message size (got ZERO)");
00097         status_code = FTCD_COMM_FAILED_TO_READ_MESSAGE_SIZE;
00098         return status_code;
00099     }
00100 
00101     //read message
00102     message = (uint8_t *)fcc_malloc(message_size);
00103     if (message == NULL) {
00104         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed to allocate message buffer");
00105         status_code = FTCD_COMM_MEMORY_OUT;
00106         return status_code;
00107     }
00108     success = read_message(message, message_size);
00109     if (!success) {
00110         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed getting message bytes");
00111         status_code = FTCD_COMM_FAILED_TO_READ_MESSAGE_BYTES;
00112         fcc_free(message);
00113         return status_code;
00114     }
00115 
00116     if (_use_signature == true) {
00117         //read message signature
00118 
00119         uint8_t sig_from_message[CS_SHA256_SIZE];
00120         success = read_message_signature(sig_from_message, sizeof(sig_from_message));
00121         if (!success) {
00122             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed getting signature bytes");
00123             status_code = FTCD_COMM_FAILED_TO_READ_MESSAGE_SIGNATURE;
00124             fcc_free(message);
00125             return status_code;
00126         }
00127 
00128         //calculate message signature
00129         uint8_t self_calculated_sig[CS_SHA256_SIZE];
00130         kcm_status_e  kcm_status = KCM_STATUS_SUCCESS;
00131         kcm_status = cs_hash(CS_SHA256, message, message_size, self_calculated_sig, sizeof(self_calculated_sig));
00132         if (kcm_status != KCM_STATUS_SUCCESS) {
00133             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed calculating message signature");
00134             status_code = FTCD_COMM_FAILED_TO_CALCULATE_MESSAGE_SIGNATURE;
00135             fcc_free(message);
00136             return status_code;
00137         }
00138 
00139         //compare signatures
00140         if (memcmp(self_calculated_sig, sig_from_message, CS_SHA256_SIZE) != 0) {
00141             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Inconsistent message signature");
00142             status_code = FTCD_COMM_INCONSISTENT_MESSAGE_SIGNATURE;
00143             fcc_free(message);
00144             return status_code;
00145         }
00146     }
00147 
00148     *message_out = message;
00149     *message_size_out = message_size;
00150     return status_code;
00151 }
00152 
00153 ftcd_comm_status_e FtcdCommBase::send_response(const uint8_t *response_message, uint32_t response_message_size)
00154 {
00155     return _send_response(response_message, response_message_size, false, FTCD_COMM_STATUS_SUCCESS);
00156 }
00157 
00158 ftcd_comm_status_e FtcdCommBase::send_response(const uint8_t *response_message, uint32_t response_message_size, ftcd_comm_status_e status_code)
00159 {
00160     return _send_response(response_message, response_message_size, true, status_code);
00161 }
00162 
00163 ftcd_comm_status_e FtcdCommBase::_send_response(const uint8_t *response_message, uint32_t response_message_size, bool send_status_code, ftcd_comm_status_e status_code)
00164 {
00165     uint32_t response_size = 0;
00166     if (_use_token == true) {
00167         response_size += (uint32_t)sizeof(uint64_t); // TOKEN
00168     }
00169     if (send_status_code == true) {
00170         response_size += (uint32_t)sizeof(uint32_t); // STATUS
00171     }
00172     if (status_code == FTCD_COMM_STATUS_SUCCESS) {
00173         response_size += (uint32_t)sizeof(uint32_t); // MESSAGE SIZE
00174         response_size += response_message_size; // MESSAGE DATA
00175         if (_use_signature == true) {
00176             response_size += CS_SHA256_SIZE; // SIGNATURE
00177         }
00178     }
00179 
00180     uint8_t *response = (uint8_t *)fcc_malloc(response_size);
00181     if (response == NULL) {
00182         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed to allocate response message buffer");
00183         status_code = FTCD_COMM_MEMORY_OUT;
00184         return status_code;
00185     }
00186 
00187     uint32_t offset = 0;
00188 
00189     if (_use_token == true) {
00190         // TOKEN
00191         memcpy(response, _header_token, FTCD_MSG_HEADER_TOKEN_SIZE_BYTES);
00192         offset = FTCD_MSG_HEADER_TOKEN_SIZE_BYTES;
00193     }
00194 
00195     if (send_status_code == true) {
00196         //STATUS
00197         uint32_t aligned_status_code = static_cast<uint32_t>(status_code);
00198         if (_network_endianness == FTCD_COMM_NET_ENDIANNESS_LITTLE) {
00199             aligned_status_code = pv_h_to_le32(aligned_status_code);
00200         } else { // big endian
00201             aligned_status_code = pv_h_to_be32(aligned_status_code);
00202         }
00203         memcpy(response + offset, &aligned_status_code, sizeof(uint32_t));
00204         offset += (uint32_t)sizeof(status_code);
00205     }
00206 
00207     if (status_code == FTCD_COMM_STATUS_SUCCESS) {
00208 
00209         if (response_message != NULL && response_message_size > 0) {
00210             // MESSAGE SIZE
00211             uint32_t aligned_msg_size = response_message_size;
00212             if (_network_endianness == FTCD_COMM_NET_ENDIANNESS_LITTLE) {
00213                 aligned_msg_size = pv_h_to_le32(aligned_msg_size);
00214             } else { // big endian
00215                 aligned_msg_size = pv_h_to_be32(aligned_msg_size);
00216             }
00217             memcpy(response + offset, &aligned_msg_size, sizeof(uint32_t));
00218             offset += (uint32_t)sizeof(uint32_t);
00219 
00220             // MESSAGE DATA
00221             memcpy(response + offset, response_message, response_message_size);
00222             offset += response_message_size;
00223 
00224         } else {
00225             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Invalid response message");
00226         }
00227 
00228         if (_use_signature == true) {
00229             kcm_status_e  kcm_status = KCM_STATUS_SUCCESS;
00230             uint8_t sig[CS_SHA256_SIZE];
00231 
00232             kcm_status = cs_hash(CS_SHA256, response_message, response_message_size, sig, CS_SHA256_SIZE);
00233             if (kcm_status != KCM_STATUS_SUCCESS) {
00234                 mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed calculating response message signature");
00235                 fcc_free(response);
00236                 return FTCD_COMM_INTERNAL_ERROR;
00237             }
00238 
00239             // SIGNATURE
00240             memcpy(response + offset, sig, CS_SHA256_SIZE);
00241         }
00242     }
00243 
00244     // Send the response...
00245     bool success = send(response, response_size);
00246 
00247     fcc_free(response);
00248 
00249     if (!success) {
00250         return FTCD_COMM_FAILED_TO_SEND_VALID_RESPONSE;
00251     }
00252     return FTCD_COMM_STATUS_SUCCESS;
00253 }
00254 
00255