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_STATUS_SUCCESS) {
00080             if (status_code != FTCD_COMM_NETWORK_CONNECTION_CLOSED) {
00081                 mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Network error (%d)", status_code);
00082             }
00083             return status_code;
00084         } 
00085     }
00086 
00087     // Read message size
00088     message_size = read_message_size();
00089     if (_network_endianness == FTCD_COMM_NET_ENDIANNESS_LITTLE) {
00090         message_size = pv_le32_to_h(message_size);
00091     } else { // big endian
00092         message_size = pv_be32_to_h(message_size);
00093     }
00094     if (message_size == 0) {
00095         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Unable to read message size (got ZERO)");
00096         status_code = FTCD_COMM_FAILED_TO_READ_MESSAGE_SIZE;
00097         return status_code;
00098     }
00099 
00100     //read message
00101     message = (uint8_t *)fcc_malloc(message_size);
00102     if (message == NULL) {
00103         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed to allocate message buffer");
00104         status_code = FTCD_COMM_MEMORY_OUT;
00105         return status_code;
00106     }
00107     success = read_message(message, message_size);
00108     if (!success) {
00109         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed getting message bytes");
00110         status_code = FTCD_COMM_FAILED_TO_READ_MESSAGE_BYTES;
00111         fcc_free(message);
00112         return status_code;
00113     }
00114 
00115     if (_use_signature == true) {
00116         //read message signature
00117 
00118         uint8_t sig_from_message[CS_SHA256_SIZE];
00119         success = read_message_signature(sig_from_message, sizeof(sig_from_message));
00120         if (!success) {
00121             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed getting signature bytes");
00122             status_code = FTCD_COMM_FAILED_TO_READ_MESSAGE_SIGNATURE;
00123             fcc_free(message);
00124             return status_code;
00125         }
00126 
00127         //calculate message signature
00128         uint8_t self_calculated_sig[CS_SHA256_SIZE];
00129         kcm_status_e  kcm_status = KCM_STATUS_SUCCESS;
00130         kcm_status = cs_hash(CS_SHA256, message, message_size, self_calculated_sig, sizeof(self_calculated_sig));
00131         if (kcm_status != KCM_STATUS_SUCCESS) {
00132             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed calculating message signature");
00133             status_code = FTCD_COMM_FAILED_TO_CALCULATE_MESSAGE_SIGNATURE;
00134             fcc_free(message);
00135             return status_code;
00136         }
00137 
00138         //compare signatures
00139         if (memcmp(self_calculated_sig, sig_from_message, CS_SHA256_SIZE) != 0) {
00140             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Inconsistent message signature");
00141             status_code = FTCD_COMM_INCONSISTENT_MESSAGE_SIGNATURE;
00142             fcc_free(message);
00143             return status_code;
00144         }
00145     }
00146 
00147     *message_out = message;
00148     *message_size_out = message_size;
00149     return status_code;
00150 }
00151 
00152 ftcd_comm_status_e FtcdCommBase::send_response(const uint8_t *response_message, uint32_t response_message_size)
00153 {
00154     return _send_response(response_message, response_message_size, false, FTCD_COMM_STATUS_SUCCESS);
00155 }
00156 
00157 ftcd_comm_status_e FtcdCommBase::send_response(const uint8_t *response_message, uint32_t response_message_size, ftcd_comm_status_e status_code)
00158 {
00159     return _send_response(response_message, response_message_size, true, status_code);
00160 }
00161 
00162 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)
00163 {
00164     uint32_t response_size = 0;
00165     if (_use_token == true) {
00166         response_size += (uint32_t)sizeof(uint64_t); // TOKEN
00167     }
00168     if (send_status_code == true) {
00169         response_size += (uint32_t)sizeof(uint32_t); // STATUS
00170     }
00171     if (status_code == FTCD_COMM_STATUS_SUCCESS) {
00172         response_size += (uint32_t)sizeof(uint32_t); // MESSAGE SIZE
00173         response_size += response_message_size; // MESSAGE DATA
00174         if (_use_signature == true) {
00175             response_size += CS_SHA256_SIZE; // SIGNATURE
00176         }
00177     }
00178 
00179     uint8_t *response = (uint8_t *)fcc_malloc(response_size);
00180     if (response == NULL) {
00181         mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed to allocate response message buffer");
00182         status_code = FTCD_COMM_MEMORY_OUT;
00183         return status_code;
00184     }
00185 
00186     uint32_t offset = 0;
00187 
00188     if (_use_token == true) {
00189         // TOKEN
00190         memcpy(response, _header_token, FTCD_MSG_HEADER_TOKEN_SIZE_BYTES);
00191         offset = FTCD_MSG_HEADER_TOKEN_SIZE_BYTES;
00192     }
00193 
00194     if (send_status_code == true) {
00195         //STATUS
00196         uint32_t aligned_status_code = static_cast<uint32_t>(status_code);
00197         if (_network_endianness == FTCD_COMM_NET_ENDIANNESS_LITTLE) {
00198             aligned_status_code = pv_h_to_le32(aligned_status_code);
00199         } else { // big endian
00200             aligned_status_code = pv_h_to_be32(aligned_status_code);
00201         }
00202         memcpy(response + offset, &aligned_status_code, sizeof(uint32_t));
00203         offset += (uint32_t)sizeof(status_code);
00204     }
00205 
00206     if (status_code == FTCD_COMM_STATUS_SUCCESS) {
00207 
00208         if (response_message != NULL && response_message_size > 0) {
00209             // MESSAGE SIZE
00210             uint32_t aligned_msg_size = response_message_size;
00211             if (_network_endianness == FTCD_COMM_NET_ENDIANNESS_LITTLE) {
00212                 aligned_msg_size = pv_h_to_le32(aligned_msg_size);
00213             } else { // big endian
00214                 aligned_msg_size = pv_h_to_be32(aligned_msg_size);
00215             }
00216             memcpy(response + offset, &aligned_msg_size, sizeof(uint32_t));
00217             offset += (uint32_t)sizeof(uint32_t);
00218 
00219             // MESSAGE DATA
00220             memcpy(response + offset, response_message, response_message_size);
00221             offset += response_message_size;
00222 
00223         } else {
00224             mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Invalid response message");
00225         }
00226 
00227         if (_use_signature == true) {
00228             kcm_status_e  kcm_status = KCM_STATUS_SUCCESS;
00229             uint8_t sig[CS_SHA256_SIZE];
00230 
00231             kcm_status = cs_hash(CS_SHA256, response_message, response_message_size, sig, CS_SHA256_SIZE);
00232             if (kcm_status != KCM_STATUS_SUCCESS) {
00233                 mbed_tracef(TRACE_LEVEL_CMD, TRACE_GROUP, "Failed calculating response message signature");
00234                 fcc_free(response);
00235                 return FTCD_COMM_INTERNAL_ERROR;
00236             }
00237 
00238             // SIGNATURE
00239             memcpy(response + offset, sig, CS_SHA256_SIZE);
00240         }
00241     }
00242 
00243     // Send the response...
00244     bool success = send(response, response_size);
00245 
00246     fcc_free(response);
00247 
00248     if (!success) {
00249         return FTCD_COMM_FAILED_TO_SEND_VALID_RESPONSE;
00250     }
00251     return FTCD_COMM_STATUS_SUCCESS;
00252 }
00253 
00254