SSBootloaderComm.cpp

00001 /*******************************************************************************
00002  * Copyright (C) 2017 Maxim Integrated Products, Inc., All Rights Reserved.
00003  *
00004  * Permission is hereby granted, free of charge, to any person obtaining a
00005  * copy of this software and associated documentation files (the "Software"),
00006  * to deal in the Software without restriction, including without limitation
00007  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
00008  * and/or sell copies of the Software, and to permit persons to whom the
00009  * Software is furnished to do so, subject to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included
00012  * in all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00015  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00016  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00017  * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
00018  * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00019  * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00020  * OTHER DEALINGS IN THE SOFTWARE.
00021  *
00022  * Except as contained in this notice, the name of Maxim Integrated
00023  * Products, Inc. shall not be used except as stated in the Maxim Integrated
00024  * Products, Inc. Branding Policy.
00025  *
00026  * The mere transfer of this software does not imply any licenses
00027  * of trade secrets, proprietary technology, copyrights, patents,
00028  * trademarks, maskwork rights, or any other form of intellectual
00029  * property whatsoever. Maxim Integrated Products, Inc. retains all
00030  * ownership rights.
00031  *******************************************************************************
00032  */
00033 
00034 #include "DSInterface.h"
00035 #include "SSBootloaderComm.h"
00036 
00037 #include "../../../../Utilities/mxm_assert.h"
00038 #include "SSInterface.h"
00039 #include "Peripherals.h"
00040 #include "utils.h"
00041 #include "BLE_ICARUS.h"
00042 
00043 
00044 static bool flag_image_on_ram = false;
00045 
00046 static const char* const cmd_tbl[] = {
00047     "bootldr",
00048     "exit",
00049     "reset",
00050     "page_size",
00051     "num_pages",
00052     "set_iv",
00053     "set_auth",
00054     "erase",
00055     "page_erase",
00056     "flash",
00057     "set_cfg bl enter_mode",
00058     "set_cfg bl enter_pin",
00059     "set_cfg bl enter_pol",
00060     "set_cfg bl exit_mode",
00061     "set_cfg bl exit_to",
00062     "set_cfg bl save",
00063     "image_on_ram",
00064     "set_cfg host ebl",
00065     "set_cfg host cdf", /* bootloader comm_delay_factor */
00066     "get_cfg host",
00067 };
00068 
00069 SSBootloaderComm::SSBootloaderComm(USBSerial *USB, SSInterface* ssInterface, DSInterface* dsInterface)
00070     :SensorComm("bl", false), m_USB(USB), ss_int(ssInterface), ds_int(dsInterface)
00071 {
00072     bl_comm_delay_factor = 1;
00073 }
00074 
00075 int SSBootloaderComm::parse_auth(const char* cmd, uint8_t *auth_bytes)
00076 {
00077     char cmdStr[] = "set_auth ";
00078     int length = strlen(cmd);
00079     int expected_length = strlen(cmdStr) + 2*AES_AUTH_SIZE;
00080     if (length != expected_length) {
00081         pr_err("Couldn't parse Auth bytes, incorrect number of characters (len:%d, expected:%d)\n",
00082             length, expected_length);
00083         return COMM_INVALID_PARAM;
00084     }
00085 
00086     const char* macPtr = cmd + strlen(cmdStr);
00087 
00088     int num_found;
00089     int byteVal;
00090     for (int aidx = 0; aidx < AES_AUTH_SIZE; aidx++) {
00091         num_found = sscanf(macPtr, "%2X", &byteVal);
00092 
00093         if (num_found != 1 || byteVal > 0xFF) {
00094             pr_err("Couldn't parse byte %d of Auth\n", aidx);
00095             return COMM_INVALID_PARAM;
00096         }
00097 
00098         auth_bytes[aidx] = (uint8_t)byteVal;
00099         macPtr += 2;
00100     }
00101 
00102     return COMM_SUCCESS;
00103 }
00104 
00105 int SSBootloaderComm::parse_iv(const char* cmd, uint8_t* iv_bytes)
00106 {
00107     char cmdStr[] = "set_iv ";
00108     int length = strlen(cmd);
00109     int expected_length = strlen(cmdStr) + 2*AES_NONCE_SIZE;
00110     if (length != expected_length) {
00111         pr_err("Couldn't parse IV, incorrect number of characters (len:%d, expected:%d)\n",
00112                length, expected_length);
00113         return COMM_INVALID_PARAM;
00114     }
00115 
00116     const char* ivPtr = cmd + strlen(cmdStr);
00117 
00118     int num_found;
00119     int byteVal;
00120     for (int ividx = 0; ividx < AES_NONCE_SIZE; ividx++) {
00121         num_found = sscanf(ivPtr, "%2X", &byteVal);
00122 
00123         if (num_found != 1 || byteVal > 0xFF) {
00124             pr_err("Couldn't parse byte %d of IV\n", ividx);
00125             return COMM_INVALID_PARAM;
00126         }
00127         iv_bytes[ividx] = (uint8_t)byteVal;
00128         ivPtr += 2;
00129     }
00130 
00131     return COMM_SUCCESS;
00132 }
00133 
00134 bool SSBootloaderComm::parse_command(const char* cmd)
00135 {
00136     int ret = EXIT_SUCCESS;
00137     bool recognizedCmd = false;
00138     char charbuf[768];
00139     int data_len = 0;
00140     bool flash_flag=false;
00141 
00142     if (!ss_int) {
00143         pr_err("No SmartSensor Interface defined!");
00144         return false;
00145     }
00146     if (!ds_int) {
00147         pr_err("No DeviceStudio Interface defined!");
00148         return false;
00149     }
00150 
00151     for (int i = 0; i < NUM_CMDS; i++) {
00152         if (starts_with(cmd, cmd_tbl[i])) {
00153             cmd_state_t user_cmd = (cmd_state_t)i;
00154             recognizedCmd = true;
00155 
00156             switch (user_cmd) {
00157                 case cmd_enter_bootldr:
00158                 {
00159                     SS_STATUS status;
00160                     status = ss_int->reset_to_bootloader();
00161                     if (status == SS_SUCCESS){
00162                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00163                         m_USB->printf(charbuf);
00164                     }
00165                     else{
00166                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00167                         m_USB->printf(charbuf);
00168                     }
00169 
00170                     ds_int->set_fw_platform(ss_int->get_ss_platform_name());
00171                     ds_int->set_fw_version(ss_int->get_ss_fw_version());
00172                     got_page_size = false;
00173                     sent_num_pages = false;
00174                 } break;
00175 
00176                 case cmd_exit_bootldr:
00177                 {
00178                     SS_STATUS status = ss_int->reset_to_main_app();
00179                     if (status == SS_SUCCESS){
00180                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00181                         m_USB->printf(charbuf);
00182                     }
00183                     else{
00184                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00185                         m_USB->printf(charbuf);
00186                     }
00187 
00188                     ds_int->set_fw_platform(ss_int->get_ss_platform_name());
00189                     ds_int->set_fw_version(ss_int->get_ss_fw_version());
00190 
00191         } break;
00192 
00193                 case cmd_reset:
00194                 {
00195                     SS_STATUS status = ss_int->reset();
00196                     if (status == SS_SUCCESS)
00197                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00198                     else
00199                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00200                     m_USB->printf("\r\n%s err=%d\r\n", cmd, ret);
00201 
00202                     ds_int->set_fw_platform(ss_int->get_ss_platform_name());
00203                     ds_int->set_fw_version(ss_int->get_ss_fw_version());
00204 
00205                 } break;
00206 
00207                 case cmd_page_size:
00208                 {
00209                     uint8_t cmd_bytes[] = { SS_FAM_R_BOOTLOADER, SS_CMDIDX_PAGESIZE };
00210                     uint8_t rxbuf[3];
00211 
00212                     SS_STATUS status = ss_int->read_cmd(
00213                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00214                             0, 0,
00215                             &rxbuf[0], ARRAY_SIZE(rxbuf));
00216 
00217                     if (status == SS_SUCCESS) {
00218                         //rxbuf holds page size in big-endian format
00219                         page_size = (256*(int)rxbuf[1]) + rxbuf[2];
00220                         mxm_assert_msg(page_size <= MAX_PAGE_SIZE, "Page size exceeds maximum allowed");
00221 
00222                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s value=%d err=%d\r\n", cmd, page_size, COMM_SUCCESS);
00223                         m_USB->printf(charbuf);
00224                         got_page_size = true;
00225 
00226                     } else {
00227                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00228                         m_USB->printf(charbuf);
00229                     }
00230                 } break;
00231 
00232                 case cmd_num_pages:
00233                 {
00234                     int num_tok = sscanf(cmd, "num_pages %d", &num_pages);
00235                     if (num_tok != 1) {
00236                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s value=%d err=%d\r\n", cmd, 0, COMM_INVALID_PARAM);
00237                         m_USB->printf(charbuf);
00238                         break;
00239                     }
00240 
00241                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SETNUMPAGES };
00242                     //num pages = 256*MSB + LSB
00243                     uint8_t data[] = { (uint8_t)((num_pages >> 8) & 0xFF), (uint8_t)(num_pages & 0xFF) };
00244 
00245                     SS_STATUS status = ss_int->write_cmd(
00246                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00247                             &data[0], ARRAY_SIZE(data));
00248 
00249                     if (status == SS_SUCCESS) {
00250                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00251                         m_USB->printf(charbuf);
00252                         sent_num_pages = true;
00253 
00254                     } else {
00255                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00256                         m_USB->printf(charbuf);
00257                     }
00258                 } break;
00259 
00260                 case cmd_set_iv:
00261                 {
00262                     uint8_t iv_bytes[AES_NONCE_SIZE];
00263                     ret = parse_iv(cmd, &iv_bytes[0]);
00264                     if (ret != COMM_SUCCESS) {
00265                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, ret);
00266                         m_USB->printf(charbuf);
00267                     }
00268                     else
00269                     {
00270                         uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SETIV };
00271 
00272                         SS_STATUS status = ss_int->write_cmd(
00273                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00274                             &iv_bytes[0], ARRAY_SIZE(iv_bytes));
00275 
00276                         if (status == SS_SUCCESS) {
00277                             data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00278                             m_USB->printf(charbuf);
00279 
00280                         } else {
00281                             data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00282                             m_USB->printf(charbuf);
00283                         }
00284                     }
00285                 } break;
00286 
00287                 case cmd_set_auth:
00288                 {
00289                     uint8_t auth_bytes[AES_AUTH_SIZE];
00290                     ret = parse_auth(cmd, &auth_bytes[0]);
00291                     if (ret != COMM_SUCCESS) {
00292                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, ret);
00293                         m_USB->printf(charbuf);
00294                     }
00295                     else
00296                     {
00297                         uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SETAUTH };
00298 
00299                         SS_STATUS status = ss_int->write_cmd(
00300                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00301                             &auth_bytes[0], ARRAY_SIZE(auth_bytes));
00302 
00303                         if (status == SS_SUCCESS) {
00304                             data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00305                             m_USB->printf(charbuf);
00306 
00307                         } else {
00308                             data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00309                             m_USB->printf(charbuf);
00310                         }
00311                     }
00312                 } break;
00313 
00314                 case cmd_erase:
00315                 {
00316                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_ERASE };
00317 
00318                     SS_STATUS status = ss_int->write_cmd(
00319                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00320                             0, 0,
00321                             SS_BOOTLOADER_ERASE_DELAY);
00322                     if (status == SS_SUCCESS){
00323                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00324                         m_USB->printf(charbuf);
00325                     }
00326                     else{
00327                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00328                         m_USB->printf(charbuf);
00329                     }
00330                 } break;
00331 
00332                 case cmd_page_erase:
00333                 {
00334                     int page_num_to_erase;
00335                     int num_tok = sscanf(cmd, "page_erase %d", &page_num_to_erase);
00336                     if (num_tok != 1) {
00337                         m_USB->printf("\r\n%s value=%d err=%d\r\n", cmd, 0, COMM_INVALID_PARAM);
00338                         break;
00339                     }
00340 
00341                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_ERASE_PAGE };
00342                     //num pages = 256*MSB + LSB
00343                     uint8_t data[] = { (uint8_t)((page_num_to_erase >> 8) & 0xFF), (uint8_t)(page_num_to_erase & 0xFF) };
00344 
00345                     SS_STATUS status = ss_int->write_cmd(
00346                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00347                             &data[0], ARRAY_SIZE(data), 50);
00348 
00349             if (status == SS_SUCCESS) {
00350                 m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00351                 sent_num_pages = true;
00352             } else {
00353                 m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00354             }
00355         } break;
00356 
00357                 case cmd_flash:
00358                 {
00359                     if (got_page_size && sent_num_pages) {
00360                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00361                         m_USB->printf(charbuf);
00362 
00363                         if (BLE::Instance().gap().getState().connected) {
00364                             BLE_Icarus_AddtoQueue((uint8_t *)charbuf, (int32_t)sizeof(charbuf), data_len);
00365                             BLE_Icarus_TransferDataFromQueue();
00366                         }
00367                         BLE::Instance().waitForEvent();
00368 
00369                         flash_page_data();
00370                         flash_flag=true;
00371                     } else {
00372                         pr_err("Can't enter flash mode. Need number of pages, and size of page"
00373                                 "(num_pages, page_size, commands)\r\n");
00374                         data_len = snprintf(charbuf, sizeof(charbuf), "\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00375                         m_USB->printf(charbuf);
00376                     }
00377         } break;
00378 
00379                 case cmd_setcfg_bl_enter_mode:
00380                 {
00381                     uint8_t mode;
00382                     ret = parse_cmd_data(cmd, cmd_tbl[i], &mode, 1, false);
00383                     if (ret != 1) {
00384                         pr_err("parse_cmd_data=%d\r\n", ret);
00385                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00386                         break;
00387                     }
00388 
00389                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER_CFG, SS_CMDIDX_BL_ENTRY, SS_BL_CFG_ENTER_BL_MODE, mode };
00390                     SS_STATUS status = ss_int->write_cmd(
00391                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00392                             0, 0);
00393                     if (status == SS_SUCCESS)
00394                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00395                     else
00396                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00397 
00398         } break;
00399 
00400                 case cmd_setcfg_bl_ebl_pin:
00401                 {
00402                     uint8_t pin[2];
00403                     ret = parse_cmd_data(cmd, cmd_tbl[i], &pin[0], 2, false);
00404                     if (ret != 2) {
00405                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00406                         break;
00407                     }
00408 
00409                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER_CFG, SS_CMDIDX_BL_ENTRY, SS_BL_CFG_EBL_PIN,
00410                                             pin[0], pin[1]};
00411                     SS_STATUS status = ss_int->write_cmd(
00412                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00413                             0, 0);
00414                     if (status == SS_SUCCESS)
00415                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00416                     else
00417                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00418 
00419         } break;
00420 
00421                 case cmd_setcfg_bl_ebl_pol:
00422                 {
00423                     uint8_t mode;
00424                     ret = parse_cmd_data(cmd, cmd_tbl[i], &mode, 1, false);
00425                     if (ret != 1) {
00426                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00427                         break;
00428                     }
00429 
00430                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER_CFG, SS_CMDIDX_BL_ENTRY, SS_BL_CFG_EBL_POL, mode };
00431                     SS_STATUS status = ss_int->write_cmd(
00432                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00433                             0, 0);
00434                     if (status == SS_SUCCESS)
00435                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00436                     else
00437                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00438 
00439         } break;
00440 
00441                 case cmd_setcfg_bl_exit_mode:
00442                 {
00443                     uint8_t mode;
00444                     ret = parse_cmd_data(cmd, cmd_tbl[i], &mode, 1, false);
00445                     if (ret != 1) {
00446                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00447                         break;
00448                     }
00449 
00450                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER_CFG, SS_CMDIDX_BL_EXIT, SS_BL_CFG_EXIT_BL_MODE, mode };
00451                     SS_STATUS status = ss_int->write_cmd(
00452                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00453                             0, 0);
00454                     if (status == SS_SUCCESS)
00455                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00456                     else
00457                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00458 
00459                 } break;
00460                 case cmd_setcfg_bl_timeout:
00461                 {
00462                     uint8_t to;
00463                     ret = parse_cmd_data(cmd, cmd_tbl[i], &to, 1, false);
00464                     if (ret != 1) {
00465                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00466                         break;
00467                     }
00468 
00469                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER_CFG, SS_CMDIDX_BL_EXIT, SS_BL_CFG_TIMEOUT, to };
00470                     SS_STATUS status = ss_int->write_cmd(
00471                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00472                             0, 0);
00473                     if (status == SS_SUCCESS)
00474                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00475                     else
00476                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00477 
00478                 } break;
00479                 case cmd_setcfg_bl_save:
00480                 {
00481                     uint8_t cmd_bytes[] = { SS_FAM_W_BOOTLOADER_CFG, SS_CMDIDX_BL_SAVE };
00482 
00483                     SS_STATUS status = ss_int->write_cmd(
00484                             &cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
00485                             0, 0, 50);
00486                     if (status == SS_SUCCESS)
00487                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00488                     else
00489                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_GENERAL_ERROR);
00490 
00491         } break;
00492                 case cmd_image_on_ram:
00493                 {
00494                     uint8_t mode = 0;
00495                     ret = parse_cmd_data(cmd, cmd_tbl[i], &mode, 1, false);
00496                     if (ret != 1) {
00497                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00498                         break;
00499                     }
00500 
00501                     /* TODO: Implement effects of this */
00502                     flag_image_on_ram = !!mode;
00503                     pr_info("flag_image_on_ram: %d, mode: %d\n", flag_image_on_ram, mode);
00504                     m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00505                     break;
00506                 } break;
00507                 case cmd_setcfg_host_ebl:
00508                 {
00509                     uint8_t tmp; /* 0: Command bootloader, 1: No command, trigger GPIO */
00510 
00511                     int ret = parse_cmd_data(cmd, cmd_tbl[i], &tmp, 1, false);
00512                     if (ret != 1) {
00513                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00514                         break;
00515                     }
00516 
00517                     if (ss_int->set_ebl_mode(tmp) == SS_SUCCESS)
00518                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00519                     else
00520                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00521                     break;
00522                 } break;
00523                 case cmd_setcfg_host_cdf:
00524                 {
00525                     uint8_t tmp;
00526 
00527                     int ret = parse_cmd_data(cmd, cmd_tbl[i], &tmp, 1, false);
00528                     if (ret != 1) {
00529                         m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_INVALID_PARAM);
00530                         break;
00531                     }
00532 
00533                     /* TODO: Implement effects of this */
00534                     bl_comm_delay_factor = tmp;
00535                     m_USB->printf("\r\n%s err=%d\r\n", cmd, COMM_SUCCESS);
00536                     break;
00537                 } break;
00538                 case cmd_getcfg_host:
00539                 {
00540                     m_USB->printf("\r\n%s ebl=%d cdf=%d err=%d\r\n", cmd, ss_int->get_ebl_mode(), bl_comm_delay_factor, COMM_SUCCESS);
00541                 } break;
00542                 default:
00543                 {
00544                     mxm_assert_msg(false, "Invalid switch case!");
00545                 }
00546             }
00547 
00548 
00549                 if (BLE::Instance().gap().getState().connected) {
00550                     if(flash_flag==false)
00551                     {
00552                         BLE_Icarus_AddtoQueue((uint8_t *)charbuf, (int32_t)sizeof(charbuf), data_len);
00553                     }
00554                 }
00555 
00556 
00557         }
00558     }
00559 
00560     return recognizedCmd;
00561 }
00562 
00563 void SSBootloaderComm::flash_page_data(void)
00564 {
00565     int totalBytes = 0;
00566     int currentPage = 1;
00567     char charbuf_flash[256];
00568     int data_len_flash = 0;
00569 
00570     static uint8_t tx_buf[MAX_PAGE_SIZE + CHECKBYTES_SIZE + 2] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SENDPAGE };
00571     uint8_t *data_buffer = &tx_buf[2];
00572 
00573     while (currentPage <= num_pages) {
00574         pr_info("Waiting for page %d/%d data (%d bytes)...", currentPage, num_pages, page_size);
00575 
00576         //Collect page data + checksum from PC/Android
00577 //      totalBytes = 100;
00578         while (totalBytes < (page_size + CHECKBYTES_SIZE)) {
00579             data_buffer[totalBytes++] = m_USB->_getc();
00580             //Here we should be able to take the data over BLE
00581         }
00582 
00583         pr_info("Done\r\n");
00584 
00585         //Send data to SmartSensor
00586         SS_STATUS status = ss_int->write_cmd(tx_buf, page_size + CHECKBYTES_SIZE + 2, 2000 /*bl_comm_delay_factor * PAGE_WRITE_DELAY_MS*/);
00587         pr_err("status: %d\r\n", status);
00588 
00589         //Inform PC/Andoid of status
00590         if (status == SS_ERR_BTLDR_CHECKSUM) {
00591             pr_err("Verify checksum failed!\r\n");
00592             data_len_flash = snprintf(charbuf_flash, sizeof(charbuf_flash), "\r\npageFlashDone err=%d\r\n", FLASH_ERR_CHECKSUM);
00593             m_USB->printf(charbuf_flash);
00594         } else if (status != SS_SUCCESS) {
00595             pr_err("Page flash failed!\r\n");
00596             data_len_flash = snprintf(charbuf_flash, sizeof(charbuf_flash), "\r\npageFlashDone err=%d\r\n", FLASH_ERR_GENERAL);
00597             m_USB->printf(charbuf_flash);
00598         } else {
00599             currentPage++;
00600             pr_err("Page flash successful!\r\n");
00601             data_len_flash = snprintf(charbuf_flash, sizeof(charbuf_flash), "\r\npageFlashDone err=%d\r\n", COMM_SUCCESS);
00602             m_USB->printf(charbuf_flash);
00603         }
00604 
00605         totalBytes = 0;
00606 
00607                 if (BLE::Instance().gap().getState().connected) {
00608                     BLE_Icarus_AddtoQueue((uint8_t *)charbuf_flash, (int32_t)sizeof(charbuf_flash), data_len_flash);
00609                     BLE_Icarus_TransferDataFromQueue();
00610                 }
00611                 BLE::Instance().waitForEvent();
00612 
00613 
00614     }
00615 }