text: /*
   * Copyright (c) 2019 ARM Limited
   * Copyright (c) 2019 STMicroelectronics
   * SPDX-License-Identifier: Apache-2.0
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   *     http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  
  #include <stdio.h>
  #include "ble/common/blecommon.h"
  #include "ble/driver/CordioHCIDriver.h"
  #include "ble/driver/CordioHCITransportDriver.h"
  #include "rtos/Semaphore.h"
  #include "hci_api.h"
  #include "hci_cmd.h"
  #include "hci_core.h"
  #include "dm_api.h"
  #include "bstream.h"
  #include "hci_mbed_os_adaptation.h"
  #include "mbed_trace.h"
  
  /* STM32WB include files */
  #include "stm32wbxx_ll_ipcc.h"
  #include "stm32wbxx_ll_system.h"
  #include "ble_bufsize.h"
  #include "tl.h"
  #include "shci.h"
  #include "shci_tl.h"
  #include "hw.h"
  #include "app_conf.h"
  #include "otp.h"
  
  /* mbed trace feature is supported */
  /* ex in mbed_app.json */
  /*   "mbed-trace.enable": "1" */
  #define TRACE_GROUP "BLWB"
  
  /******************************************************************************
   * BLE config parameters
   ******************************************************************************/
  /*  Defined from WB Cube reference SW */
  #define CFG_TLBLE_EVT_QUEUE_LENGTH 5
  #define CFG_TLBLE_MOST_EVENT_PAYLOAD_SIZE 255   /**< Set to 255 with the memory manager and the mailbox */
  #define TL_BLE_EVENT_FRAME_SIZE ( TL_EVT_HDR_SIZE + CFG_TLBLE_MOST_EVENT_PAYLOAD_SIZE )
  #define POOL_SIZE (CFG_TLBLE_EVT_QUEUE_LENGTH*4*DIVC(( sizeof(TL_PacketHeader_t) + TL_BLE_EVENT_FRAME_SIZE ), 4))
  
  #define CONFIG_DATA_PUBADDR_OFFSET          (0x00) /**< Bluetooth public address */
  #define CONFIG_DATA_PUBADDR_LEN             (6)
  
  /* HCI related defines */
  #define HCI_RESET_RAND_CNT        4
  #define VENDOR_SPECIFIC_EVENT     0xFF
  #define ACI_HAL_SET_TX_POWER_LEVEL 0xFC0F
  #define ACI_WRITE_CONFIG_DATA_OPCODE 0xFC0C
  #define ACI_READ_CONFIG_DATA_OPCODE 0xFC0D
  #define MAX_HCI_ACL_PACKET_SIZE (sizeof(TL_PacketHeader_t) + 5 + 251)
  #define MAX_HACI_EVT_SIZE (255+5)
  
  /* mbed_trace: debug traces (tr_debug) can be disabled here with no change in mbed_app.json */
  // #undef TRACE_LEVEL_DEBUG
  // #define TRACE_LEVEL_DEBUG 0
  
  /* tr_debug : add data content in console print */
  #define PRINT_HCI_DATA 0
  
  /******************************************************************************
   * BLE config parameters
   ******************************************************************************/
  static void evt_received(TL_EvtPacket_t *hcievt);
  static void syscmd_status_not(SHCI_TL_CmdStatus_t status);
  static void sysevt_received(void *pdata);
  static void acl_data_ack(void);
  static bool acl_data_wait(void);
  static void init_debug(void);
  static bool get_bd_address(uint8_t *bd_addr);
  static bool sysevt_wait(void);
  static bool sysevt_check(void);
  
  extern int BLE_inited;
  
  namespace ble {
  namespace vendor {
  namespace stm32wb {
  
  /**
   * stm32wb HCI driver implementation
   * @see CordioHCIDriver
   */
  class HCIDriver : public CordioHCIDriver {
  public:
      /**
       * Construction of the HCIDriver.
       * @param transport: Transport of the HCI commands.
       * @param rst: Name of the reset pin
       */
      HCIDriver(
          CordioHCITransportDriver &transport_driver
      ) : CordioHCIDriver(transport_driver) { }
  
      virtual buf_pool_desc_t get_buffer_pool_description();
      /**
       * @see CordioHCIDriver::do_initialize
       */
      virtual void do_initialize()
      {
          // Nothig needed, init is only at transpot layer level
      }
  
      /**
       * @see CordioHCIDriver::do_terminate
       */
      virtual void do_terminate()
      {
          // Nothig needed, init is only at transpot layer level
      }
  
      /**
      * @see CordioHCIDriver::start_reset_sequence
      */
      virtual void start_reset_sequence()
      {
          /* send an HCI Reset command to start the sequence */
          HciResetCmd();
      }
  
      static uint8_t convert_db_to_tx_power_index(int8_t level_db) {
          const int8_t conversion[] = {
              -40, -21, -20, -19,
              -18, -16, -15, -14,
              -13, -12, -11, -10,
               -9,  -8,  -7,  -6,
               -5,  -4,  -3,  -2,
               -1,  -1,  -1,  -1,
                0,   0,   1,   2,
                3,   4,   5,   6
          };
  
          uint8_t index;
          for (index = 0; index < sizeof(conversion); ++index) {
              if (level_db <= conversion[index]) {
                  break;
              }
          }
          return index;
      }
  
      virtual ble_error_t set_tx_power(int8_t level_db) {
  
  
          uint8_t buf[2];
          buf[0] = 0x1; // Enable high power mode - deprecated and ignored on STM32WB
          buf[1] = convert_db_to_tx_power_index(level_db);
  
          HciVendorSpecificCmd(ACI_HAL_SET_TX_POWER_LEVEL, 2, buf);
  
          return BLE_ERROR_NONE;
      }
  
      /**
       * @see CordioHCIDriver::handle_reset_sequence
       */
      virtual void handle_reset_sequence(uint8_t *pMsg)
      {
          uint16_t       opcode;
          static uint8_t randCnt;
  
          /* if event is a command complete event */
          if (*pMsg == HCI_CMD_CMPL_EVT) {
              tr_debug("Command Complete Event Command");
  #if (PRINT_HCI_DATA)
              for (uint8_t i = 0; i < 20; i++) {
                  tr_debug("  %02X", *((uint8_t *)pMsg + i));
              }
  #endif
              /* parse parameters */
              tr_debug("  HCI_EVT_HDR_LEN=%d", HCI_EVT_HDR_LEN);
              pMsg += HCI_EVT_HDR_LEN;
              pMsg++;                   /* skip num packets */
              BSTREAM_TO_UINT16(opcode, pMsg);
              pMsg++;                   /* skip status */
  
              /* decode opcode */
              tr_debug("  opcode = %#06x", opcode);
              switch (opcode) {
                  case HCI_OPCODE_RESET:
                      /* initialize rand command count */
                      randCnt = 0;
                      tr_info("WB Reset Received");
                      /* Once reset complete evet is received we need
                       * to send a few more commands:
                       * Tx power and set bd addr
                       */
                      if (get_bd_address(bd_addr)) {
                          aciWriteConfigData(CONFIG_DATA_PUBADDR_OFFSET, bd_addr);
                          tr_info("Setting Bdaddr: %02x:%02x:%02x:%02x:%02x:%02x",
                                  bd_addr[0],
                                  bd_addr[1],
                                  bd_addr[2],
                                  bd_addr[3],
                                  bd_addr[4],
                                  bd_addr[5]);
                      } else {
                          tr_info("could not find BDaddr");
                          /*  Skip to next step */
                          set_tx_power(MBED_CONF_CORDIO_PREFERRED_TX_POWER);
                      }
                      break;
  
                  case ACI_WRITE_CONFIG_DATA_OPCODE:
                      tr_debug("Bluetooth Device address set");
                      /* set the event mask to control which events are generated by the
                       * controller for the host */
                      set_tx_power(MBED_CONF_CORDIO_PREFERRED_TX_POWER);
                      break;
  
  
                  case ACI_HAL_SET_TX_POWER_LEVEL:
                      tr_debug("Tx Power Level set");
                      //signal_reset_sequence_done();
                      HciSetEventMaskCmd((uint8_t *) hciEventMask);
                      break;
  
                  case HCI_OPCODE_SET_EVENT_MASK:
                      // set the event mask to control which LE events are generated by
                      // the controller for the host
                      HciLeSetEventMaskCmd((uint8_t *) hciLeEventMask);
                      break;
  
                  case HCI_OPCODE_LE_SET_EVENT_MASK:
                      /* below command is not supported */
  #if COMMAND_NOT_SUPPORTED_SKIP_STEP
                      // set the event mask to control which events are generated by the
                      // controller for the host (2nd page of flags )
                      HciSetEventMaskPage2Cmd((uint8_t *) hciEventMaskPage2);
                      break;
  
                  case HCI_OPCODE_SET_EVENT_MASK_PAGE2:
  #endif
                      // Ask the Bluetooth address of the controller
                      HciReadBdAddrCmd();
                      break;
  
                  case HCI_OPCODE_READ_BD_ADDR:
                      // Store the Bluetooth address in the stack runtime parameter
                      BdaCpy(hciCoreCb.bdAddr, pMsg);
                      // Read the size of the buffer of the controller
                      HciLeReadBufSizeCmd();
                      break;
  
                  case HCI_OPCODE_LE_READ_BUF_SIZE:
                      // Store the buffer parameters in the stack runtime parameters
                      BSTREAM_TO_UINT16(hciCoreCb.bufSize, pMsg);
                      BSTREAM_TO_UINT8(hciCoreCb.numBufs, pMsg);
                      /* initialize ACL buffer accounting */
                      hciCoreCb.availBufs = hciCoreCb.numBufs;
                      // read the states and state combinations supported by the link
                      // layer of the controller
                      HciLeReadSupStatesCmd();
                      break;
  
                  case HCI_OPCODE_LE_READ_SUP_STATES:
                      // store supported state and combination in the runtime parameters
                      // of the stack
                      memcpy(hciCoreCb.leStates, pMsg, HCI_LE_STATES_LEN);
                      // read the total of whitelist entries that can be stored in the
                      // controller.
                      HciLeReadWhiteListSizeCmd();
                      break;
  
                  case HCI_OPCODE_LE_READ_WHITE_LIST_SIZE:
                      // store the number of whitelist entries in the stack runtime
                      // parameters
                      BSTREAM_TO_UINT8(hciCoreCb.whiteListSize, pMsg);
  
                      // Read the LE features supported by the controller
                      HciLeReadLocalSupFeatCmd();
                      break;
  
                  case HCI_OPCODE_LE_READ_LOCAL_SUP_FEAT:
                      // Store the set of LE features supported by the controller
                      BSTREAM_TO_UINT16(hciCoreCb.leSupFeat, pMsg);
                      // read the total number of address translation entries which can be
                      // stored in the controller resolving list.
                      hciCoreReadResolvingListSize();
                      break;
  
                  case HCI_OPCODE_LE_READ_RES_LIST_SIZE:
                      // store the number of address translation entries in the stack
                      // runtime parameter
                      BSTREAM_TO_UINT8(hciCoreCb.resListSize, pMsg);
  
                      // read the Controller's maximum supported payload octets and packet
                      // duration times for transmission and reception
                      hciCoreReadMaxDataLen();
                      break;
  
                  case HCI_OPCODE_LE_READ_MAX_DATA_LEN: {
                      // store payload definition in the runtime stack parameters.
                      uint16_t maxTxOctets;
                      uint16_t maxTxTime;
  
                      BSTREAM_TO_UINT16(maxTxOctets, pMsg);
                      BSTREAM_TO_UINT16(maxTxTime, pMsg);
  
                      /* use Controller's maximum supported payload octets and packet duration times
                      * for transmission as Host's suggested values for maximum transmission number
                      * of payload octets and maximum packet transmission time for new connections.
                      */
                      HciLeWriteDefDataLen(maxTxOctets, maxTxTime);
                  }
                  break;
  
                  case HCI_OPCODE_LE_WRITE_DEF_DATA_LEN:
                      if (hciCoreCb.extResetSeq) {
                          HciReadLocalVerInfoCmd();
                      } else {
                          /* initialize extended parameters */
                          hciCoreCb.maxAdvDataLen = 0;
                          hciCoreCb.numSupAdvSets = 0;
                          hciCoreCb.perAdvListSize = 0;
                          /* send next command in sequence */
                          HciLeRandCmd();
                      }
                      break;
  
                  case HCI_OPCODE_READ_LOCAL_VER_INFO:
                  case HCI_OPCODE_LE_READ_MAX_ADV_DATA_LEN:
                  case HCI_OPCODE_LE_READ_NUM_SUP_ADV_SETS:
                  case HCI_OPCODE_LE_READ_PER_ADV_LIST_SIZE:
                      // handle extended command
                      if (hciCoreCb.extResetSeq) {
                          /* send next extended command in sequence */
                          (*hciCoreCb.extResetSeq)(pMsg, opcode);
                      }
                      break;
  
                  case HCI_OPCODE_LE_RAND:
                      /* check if need to send second rand command */
                      if (randCnt < (HCI_RESET_RAND_CNT - 1)) {
                          randCnt++;
                          HciLeRandCmd();
                      } else {
                          uint8_t addr[6] = { 0 };
                          memcpy(addr, pMsg, sizeof(addr));
                          DM_RAND_ADDR_SET(addr, DM_RAND_ADDR_STATIC);
                          // note: will invoke set rand address
                          set_random_static_address(addr);
                      }
                      break;
  
                  case HCI_OPCODE_LE_SET_RAND_ADDR:
                      /* send next command in sequence */
                      signal_reset_sequence_done();
                      break;
  
                  default:
                      tr_info("Complete Event in reset seq with unknown opcode =0x%4X", opcode);
                      break;
              }
          } else if (*pMsg == HCI_CMD_STATUS_EVT) {
              uint8_t status;
              /* get status */
              /* parse parameters */
              pMsg += HCI_EVT_HDR_LEN;
              status = *pMsg;
              pMsg++;
              pMsg++;                   /* skip num packets */
              BSTREAM_TO_UINT16(opcode, pMsg);
              tr_info("Command Status event, status:%d, opcode=0x%4X", status, opcode);
          } else {
              /**
               * vendor specific event
               */
              if (pMsg[0] == VENDOR_SPECIFIC_EVENT) {
                  /* parse parameters */
                  pMsg += HCI_EVT_HDR_LEN;
                  BSTREAM_TO_UINT16(opcode, pMsg);
                  tr_debug("Vendor specific event, opcode=0x%4X", opcode);
              } else {
                  tr_info("Unknown event %d!", pMsg[0]);
              }
          }
      }
  
  private:
      uint8_t bd_addr[6];
  
      void aciReadConfigParameter(uint8_t offset)
      {
          uint8_t *pBuf = hciCmdAlloc(ACI_READ_CONFIG_DATA_OPCODE, 1);
          if (!pBuf) {
              return;
          }
  
          pBuf[3] = offset;
          hciCmdSend(pBuf);
      }
  
      template<size_t N>
      void aciWriteConfigData(uint8_t offset, uint8_t (&buf)[N])
      {
          uint8_t *pBuf = hciCmdAlloc(ACI_WRITE_CONFIG_DATA_OPCODE, 2 + N);
          if (!pBuf) {
              return;
          }
  
          pBuf[3] = offset;
          pBuf[4] = N;
          memcpy(pBuf + 5, buf, N);
          hciCmdSend(pBuf);
      }
  
      void hciCoreReadResolvingListSize(void)
      {
          /* if LL Privacy is supported by Controller and included */
          if ((hciCoreCb.leSupFeat & HCI_LE_SUP_FEAT_PRIVACY) &&
                  (hciLeSupFeatCfg & HCI_LE_SUP_FEAT_PRIVACY)) {
              /* send next command in sequence */
              HciLeReadResolvingListSize();
          } else {
              hciCoreCb.resListSize = 0;
  
              /* send next command in sequence */
              hciCoreReadMaxDataLen();
          }
      }
  
      void hciCoreReadMaxDataLen(void)
      {
          /* if LE Data Packet Length Extensions is supported by Controller and included */
          if ((hciCoreCb.leSupFeat & HCI_LE_SUP_FEAT_DATA_LEN_EXT) &&
                  (hciLeSupFeatCfg & HCI_LE_SUP_FEAT_DATA_LEN_EXT)) {
              /* send next command in sequence */
              HciLeReadMaxDataLen();
          } else {
              /* send next command in sequence */
              HciLeRandCmd();
          }
      }
  };
  
  ble::buf_pool_desc_t ble::vendor::stm32wb::HCIDriver::get_buffer_pool_description()
  {
      // Use default buffer pool
      return ble::CordioHCIDriver::get_default_buffer_pool_description();
  }
  
  
  
  class TransportDriver : public CordioHCITransportDriver {
  public:
      TransportDriver(TL_CmdPacket_t *BleCmdBuffer, TL_CmdPacket_t *SystemCmdBuffer, uint8_t *EvtPool, uint8_t *SystemSpareEvtBuffer, uint8_t *BleSpareEvtBuffer, uint8_t *HciAclDataBuffer)
      {
  
          bleCmdBuf = BleCmdBuffer;
          sysCmdBuf = SystemCmdBuffer;
          evtPool = EvtPool;
          sysSpareEvtBuf = SystemSpareEvtBuffer;
          bleSpareEvtBuf = BleSpareEvtBuffer;
          aclDataBuffer = HciAclDataBuffer;
      }
  
      virtual ~TransportDriver() { }
  
      /**
       * @see CordioHCITransportDriver::initialize
       */
      virtual void initialize()
      {
          /*  Check whether M0 sub-system was started already by
           *  checking if the system event was already received
           *  before. If it was not, then go thru all init. */
          if (!sysevt_check()) {
              init_debug();
              stm32wb_reset();
              transport_init();
  
              WirelessFwInfo_t wireless_info_instance;
              WirelessFwInfo_t *p_wireless_info = &wireless_info_instance;
              if (SHCI_GetWirelessFwInfo(p_wireless_info) != SHCI_Success) {
                  tr_info("SHCI_GetWirelessFwInfo error");
              } else {
                  tr_info("WIRELESS COPROCESSOR FW VERSION ID = %d.%d.%d", p_wireless_info->VersionMajor, p_wireless_info->VersionMinor, p_wireless_info->VersionSub);
                  tr_info("WIRELESS COPROCESSOR FW STACK TYPE = %d", p_wireless_info->StackType);
              }
          }
      }
  
      /**
       * @see CordioHCITransportDriver::terminate
       */
      virtual void terminate() { }
  
      /**
       * @see CordioHCITransportDriver::write
       */
      virtual uint16_t write(uint8_t type, uint16_t len, uint8_t *pData)
      {
          return mbox_write(type, len, pData);
      }
  
  private:
      void transport_init(void)
      {
          TL_MM_Config_t tl_mm_config;
          TL_BLE_InitConf_t tl_ble_Config;
          /* STM32WB offers a System Channel HCI interface for
             offering system services, with proprietary commands.
             System Channel must be used as well for starting up
             BLE service so we need to initialize it. */
          SHCI_TL_HciInitConf_t shci_init_config;
  
          /**< Reference table initialization */
          TL_Init();
  
          /**< System channel initialization */
          shci_init_config.p_cmdbuffer = (uint8_t *)sysCmdBuf;
          shci_init_config.StatusNotCallBack = syscmd_status_not;
          shci_init(sysevt_received, (void *) &shci_init_config);
  
          /**< Memory Manager channel initialization */
          tl_mm_config.p_BleSpareEvtBuffer = bleSpareEvtBuf;
          tl_mm_config.p_SystemSpareEvtBuffer = sysSpareEvtBuf;
          tl_mm_config.p_AsynchEvtPool = evtPool;
          tl_mm_config.AsynchEvtPoolSize = POOL_SIZE;
          TL_MM_Init(&tl_mm_config);
  
          TL_Enable();
  
          /*  At this stage, we'll need to wait for ready event,
           *  passed thru TL_SYS_EvtReceived */
          if (!sysevt_wait()) {
              tr_info("ERROR booting WB controler");
              return;
          }
  
          // TO DO : check if we need to disable LPM
          // requires to import as well all lpm driver
          tl_ble_Config.p_AclDataBuffer = aclDataBuffer;
          tl_ble_Config.IoBusAclDataTxAck = acl_data_ack;
          tl_ble_Config.p_cmdbuffer = (uint8_t *)bleCmdBuf;
          tl_ble_Config.IoBusEvtCallBack = evt_received;
          TL_BLE_Init(&tl_ble_Config);
  
  
          /* Now start BLE service on firmware side, using Vendor specific
           * command on the System Channe
           */
          stm32wb_start_ble();
      }
  
      uint16_t mbox_write(uint8_t type, uint16_t len, uint8_t *pData)
      {
          // Note: Until enum is avalable
          // type 01 Command
          // type 02 ACL DATA
          // type 03 SCO Voice (not supported)
          // type 04 event - uplink (not suported)
          tr_debug("mbox_write type:%d, len:%d", type, len);
          /*  TO DO : MANAGE ACL DATA CASE in separate buffer */
          switch (type) {
              case 1://BLE command
                  bleCmdBuf->cmdserial.type = type; // for now this param is overwritten in TL_BLE_SendCmd
                  memcpy((void *) &bleCmdBuf->cmdserial.cmd, pData, len);
                  /* We're tracing here the command, after copy in shared mem but before
                   * * M0 trigger. */
                  tr_info("TX>> BLE CMD");
                  /* Trace the buffer including Type (+1 on lngth) */
                  tr_debug("  Type %#x", bleCmdBuf->cmdserial.type);
                  tr_debug("   Cmd %#x", bleCmdBuf->cmdserial.cmd.cmdcode);
                  tr_debug("   Len %#x", bleCmdBuf->cmdserial.cmd.plen);
  #if (PRINT_HCI_DATA)
                  for (uint8_t i = 0; i < bleCmdBuf->cmdserial.cmd.plen; i++) {
                      tr_debug("  %02X", *(((uint8_t *)&bleCmdBuf->cmdserial.cmd.payload) + i));
                  }
  #endif
                  TL_BLE_SendCmd(NULL, 0); // unused parameters for now
                  break;
              case 2://ACL DATA
                  if (!acl_data_wait()) {
                      tr_info("ERROR: previous ACL message not ACK'd");
                      /*  return number of bytes sent, 0 in this error case */
                      return 0;
                  }
                  TL_AclDataSerial_t *aclDataSerial = (TL_AclDataSerial_t *)(aclDataBuffer + sizeof(TL_PacketHeader_t));
                  aclDataSerial->type = type; // for now this param is overwritten in TL_BLE_SendCmd
                  memcpy(aclDataBuffer + + sizeof(TL_PacketHeader_t) + sizeof(type), pData, len);
                  TL_BLE_SendAclData(NULL, 0); // unused parameters for now
                  tr_info("TX>> BLE ACL");
  #if (PRINT_HCI_DATA)
                  for (uint8_t i = 0; i < len + 1 + 8; i++) {
                      tr_debug("  %02x", *(((uint8_t *) aclDataBuffer) + i));
                  }
  #endif
                  break;
          }
          return len;
      }
  
      void stm32wb_reset(void)
      {
          // Reset IPCC
          LL_AHB3_GRP1_EnableClock(LL_AHB3_GRP1_PERIPH_IPCC);
  
          LL_C1_IPCC_ClearFlag_CHx(
              IPCC,
              LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
              | LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
  
          LL_C2_IPCC_ClearFlag_CHx(
              IPCC,
              LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
              | LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
  
          LL_C1_IPCC_DisableTransmitChannel(
              IPCC,
              LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
              | LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
  
          LL_C2_IPCC_DisableTransmitChannel(
              IPCC,
              LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
              | LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
  
          LL_C1_IPCC_DisableReceiveChannel(
              IPCC,
              LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
              | LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
  
          LL_C2_IPCC_DisableReceiveChannel(
              IPCC,
              LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
              | LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
  
          /* Set IPCC default IRQ handlers */
          NVIC_SetVector(IPCC_C1_TX_IRQn, (uint32_t)HW_IPCC_Tx_Handler);
          NVIC_SetVector(IPCC_C1_RX_IRQn, (uint32_t)HW_IPCC_Rx_Handler);
  
          return;
      } // stm32wb_reset
  
      void stm32wb_start_ble(void)
      {
          SHCI_C2_Ble_Init_Cmd_Packet_t ble_init_cmd_packet = {
              0, 0, 0,                            /**< Header unused */
              0,                                  /** pBleBufferAddress not used */
              0,                                  /** BleBufferSize not used */
              CFG_BLE_NUM_GATT_ATTRIBUTES,
              CFG_BLE_NUM_GATT_SERVICES,
              CFG_BLE_ATT_VALUE_ARRAY_SIZE,
              CFG_BLE_NUM_LINK,
              CFG_BLE_DATA_LENGTH_EXTENSION,
              CFG_BLE_PREPARE_WRITE_LIST_SIZE,
              CFG_BLE_MBLOCK_COUNT,
              CFG_BLE_MAX_ATT_MTU,
              CFG_BLE_SLAVE_SCA,
              CFG_BLE_MASTER_SCA,
              CFG_BLE_LSE_SOURCE,
              CFG_BLE_MAX_CONN_EVENT_LENGTH,
              CFG_BLE_HSE_STARTUP_TIME,
              CFG_BLE_VITERBI_MODE,
              CFG_BLE_LL_ONLY,
              0                                   /** TODO Should be read from HW */
          };
          /**
           * Starts the BLE Stack on CPU2
           */
          SHCI_C2_BLE_Init(&ble_init_cmd_packet);
  
          /* Used in flash_api.c */
          BLE_inited = 1;
      }
  
      TL_CmdPacket_t *bleCmdBuf;
      TL_CmdPacket_t *sysCmdBuf;
      uint8_t *evtPool;
      uint8_t *sysSpareEvtBuf;
      uint8_t *aclDataBuffer;
      uint8_t *bleSpareEvtBuf;
  }; // class TransportDriver
  
  } // namespace stm32wb
  } // namespace vendor
  } // namespace ble
  
  /*  There must be only 1 instance of the Transport Driver in STM32WB
   *  and the command buffers needs to be located in correct memory areas
   */
  
  /* Private macros ------------------------------------------------------------*/
  /* Private variables ---------------------------------------------------------*/
  PLACE_IN_SECTION("MB_MEM1") ALIGN(4) static TL_CmdPacket_t BleCmdBuffer;
  
  PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static uint8_t HciAclDataBuffer[MAX_HCI_ACL_PACKET_SIZE];
  PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static uint8_t EvtPool[POOL_SIZE];
  PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static TL_CmdPacket_t SystemCmdBuffer;
  PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static uint8_t     SystemSpareEvtBuffer[sizeof(TL_PacketHeader_t) + TL_EVT_HDR_SIZE + 255];
  PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static uint8_t     BleSpareEvtBuffer[sizeof(TL_PacketHeader_t) + TL_EVT_HDR_SIZE + 255];
  
  
  /**
   * Cordio HCI driver factory
   */
  ble::CordioHCIDriver &ble_cordio_get_hci_driver()
  {
      static ble::vendor::stm32wb::TransportDriver transport_driver(
          &BleCmdBuffer,
          &SystemCmdBuffer,
          EvtPool,
          SystemSpareEvtBuffer,
          BleSpareEvtBuffer,
          HciAclDataBuffer
      );
  
      static ble::vendor::stm32wb::HCIDriver hci_driver(
          transport_driver /* other hci driver parameters */
      );
  
      return hci_driver;
  }
  
  static void evt_received(TL_EvtPacket_t *hcievt)
  {
      uint16_t len = 0;
  
      // We need to memcpy the data before passing to higher layers.
      switch (hcievt->evtserial.type) {
          case TL_BLEEVT_PKT_TYPE:
              len = hcievt->evtserial.evt.plen + TL_EVT_HDR_SIZE;
              ble::vendor::stm32wb::TransportDriver::on_data_received((uint8_t *)&hcievt->evtserial, len);
              break;
          case TL_ACL_DATA_PKT_TYPE: {
              TL_AclDataSerial_t *acl = &(((TL_AclDataPacket_t *)hcievt)->AclDataSerial);
              len = acl->length + 5;
              ble::vendor::stm32wb::TransportDriver::on_data_received((uint8_t *)acl, len);
          }
          break;
          default:
              // should not happen - let's block to check
              tr_error("BLE TL evt_received, wrong type:%d", hcievt->evtserial.type);
              break;
      }
  
      /*  In case Event belongs to the Evt Pool we need to inform  */
      if (((uint8_t *)hcievt >= EvtPool) && ((uint8_t *)hcievt < (EvtPool + POOL_SIZE))) {
          /*  Free the message from shared memory */
          TL_MM_EvtDone(hcievt);
      }
  }
  
  /**
   * TL Mailbox synchronisation means
   */
  
  /* Using Semaphore to implemented blocking cmd/resp on system channel */
  static rtos::Semaphore sys_event_sem(0, 1);
  static rtos::Semaphore sys_resp_sem(0, 1);
  static rtos::Semaphore acl_ack_sem(1, 1);
  
  static void acl_data_ack(void)
  {
      /**
       * The current implementation assumes the tackGUI will not send a new HCI ACL DATA packet before this ack is received
       * ( which means the CPU2 has handled the previous packet )
       * In order to implement a secure mechanism, it is required either
       * - a flow control with the stack
       * - a local pool of buffer to store packets received from the stack
       */
      acl_ack_sem.release();
      return;
  }
  
  static bool acl_data_wait(void)
  {
  
      /* Wait 10 sec for previous ACL command to be ack'ed by Low Layers
       * before sending the next one */
      if (!acl_ack_sem.try_acquire_for(10000)) {
          return false;
      } else {
          return true;
      }
  }
  
  /*  WEAK callbacks from the BLE TL driver - will be called under Interrupt */
  static void sysevt_received(void *pdata)
  {
      /* For now only READY event is received, so we know this is it */
      sys_event_sem.release();
      /* But later on ... we'll have to parse the answer */
      return;
  }
  
  /*  returns true if ssyevt was received, false otherwise */
  static bool sysevt_wait(void)
  {
      /*  Wait for 10sec max - if not return an error */
      if (!sys_event_sem.try_acquire_for(10000)) {
          return false;
      } else {
          /*  release immmediately, now that M0 runs */
          sys_event_sem.release();
          return true;
      }
  }
  
  /*  returns true if ssyevt was already received, which means M0 core is
   *  already up and running */
  static bool sysevt_check(void)
  {
      /*  Check if system is UP and runing already */
      if (!sys_event_sem.try_acquire_for(10)) {
          return false;
      } else {
          /*  release immmediately as M0 already runs */
          sys_event_sem.release();
          return true;
      }
  }
  
  static void syscmd_status_not(SHCI_TL_CmdStatus_t status)
  {
      tr_debug("syscmd_status_not, status:%d", status);
      return;
  }
  
  void shci_notify_asynch_evt(void *pdata)
  {
      /* Need to parse data in future version */
      shci_user_evt_proc();
      return;
  }
  
  void shci_cmd_resp_release(uint32_t flag)
  {
      sys_resp_sem.release();
      return;
  }
  
  void shci_cmd_resp_wait(uint32_t timeout)
  {
      /* TO DO: manage timeouts if we can return an error */
      if (!sys_resp_sem.try_acquire_for(timeout)) {
          tr_error("shci_cmd_resp_wait timed out");
      }
  }
  
  void shci_register_io_bus(tSHciIO *fops)
  {
      /* Register IO bus services */
      fops->Init    = TL_SYS_Init;
      fops->Send    = TL_SYS_SendCmd;
  }
  
  /**
   * Few utilities functions
   */
  static void init_debug(void)
  {
      /* In case of MBED debug profile, configure debugger support */
  #if (defined(MBED_DEBUG) || (CFG_DEBUGGER_SUPPORTED == 1))
      tr_info("init_debug ENABLED");
      /**
       * Keep debugger enabled while in any low power mode
       */
      HAL_DBGMCU_EnableDBGSleepMode();
      HAL_DBGMCU_EnableDBGStopMode();
      HAL_DBGMCU_EnableDBGStandbyMode();
  
      /***************** ENABLE DEBUGGER *************************************/
      LL_EXTI_EnableIT_32_63(LL_EXTI_LINE_48);
      LL_C2_EXTI_EnableIT_32_63(LL_EXTI_LINE_48);
  
  #else
      tr_info("init_debug DISABLED");
  
      GPIO_InitTypeDef gpio_config = {0};
  
      gpio_config.Pull = GPIO_NOPULL;
      gpio_config.Mode = GPIO_MODE_ANALOG;
  
      gpio_config.Pin = GPIO_PIN_15 | GPIO_PIN_14 | GPIO_PIN_13;
      __HAL_RCC_GPIOA_CLK_ENABLE();
      HAL_GPIO_Init(GPIOA, &gpio_config);
  
      gpio_config.Pin = GPIO_PIN_4 | GPIO_PIN_3;
      __HAL_RCC_GPIOB_CLK_ENABLE();
      HAL_GPIO_Init(GPIOB, &gpio_config);
  
      HAL_DBGMCU_DisableDBGSleepMode();
      HAL_DBGMCU_DisableDBGStopMode();
      HAL_DBGMCU_DisableDBGStandbyMode();
  
  #endif /* (CFG_DEBUGGER_SUPPORTED == 1) */
  
      return;
  }
  
  /* This function fills in a BD address table */
  bool get_bd_address(uint8_t *bd_addr)
  {
      uint8_t *otp_addr;
      uint32_t udn;
      uint32_t company_id;
      uint32_t device_id;
      bool bd_found;
  
      udn = LL_FLASH_GetUDN();
  
      if (udn != 0xFFFFFFFF) {
          tr_info("Found Unique Device Number: %#06x", udn);
  
          company_id = LL_FLASH_GetSTCompanyID();
          device_id = LL_FLASH_GetDeviceID();
  
          bd_addr[0] = (uint8_t)(udn & 0x000000FF);
          bd_addr[1] = (uint8_t)((udn & 0x0000FF00) >> 8);
          bd_addr[2] = (uint8_t)((udn & 0x00FF0000) >> 16);
          bd_addr[3] = (uint8_t)device_id;
          bd_addr[4] = (uint8_t)(company_id & 0x000000FF);
          bd_addr[5] = (uint8_t)((company_id & 0x0000FF00) >> 8);
  
          bd_found = true;
      } else {
          otp_addr = OTP_Read(0);
          if (otp_addr) {
              memcpy(bd_addr, ((OTP_ID0_t *)otp_addr)->bd_address, CONFIG_DATA_PUBADDR_LEN);
              bd_found = false;
          } else {
              tr_debug("Cannot find Bluetooth Device ADDRESS to program - will leave hw default");
              bd_found = true;
          }
      }
  
      return bd_found;
  }
  
  