nimble/ll: Add LE CS Set Procedure Parameters command

nimble/controller/src/ble_ll_cs.c

@@ -928,7 +928,57 @@ int
 ble_ll_cs_hci_set_proc_params(const uint8_t *cmdbuf, uint8_t cmdlen,
  uint8_t *rspbuf, uint8_t *rsplen)
 {
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_proc_params_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_proc_params_rp *rsp = (void *)rspbuf;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_config *conf;
+ struct ble_ll_cs_pref_proc_params *params;
+
+ if (cmdlen != sizeof(*cmd) || cmd->config_id >= BLE_LL_CS_CONFIG_MAX_NUM) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ /* If no connection handle exit with error */
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ conf = &connsm->cssm->config[cmd->config_id];
+
+ /* If CS configuration with Config_ID does not exists */
+ if (!conf->config_enabled) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ /* If CS measurement is enabled exit with error */
+ if (connsm->cssm->measurement_enabled) {
+ return BLE_ERR_CMD_DISALLOWED;
+ }
+
+ if (g_ble_ll_cs_chan_count < 15) {
+ return BLE_ERR_INSUFFICIENT_CHAN;
+ }
+
+ params = &conf->pref_proc_params;
+ params->max_procedure_len = htole16(cmd->max_procedure_len);
+ params->min_procedure_interval = htole16(cmd->min_procedure_interval);
+ params->max_procedure_interval = htole16(cmd->max_procedure_interval);
+ params->max_procedure_count = htole16(cmd->max_procedure_count);
+ params->min_subevent_len = get_le24(cmd->min_subevent_len);
+ params->max_subevent_len = get_le24(cmd->max_subevent_len);
+ params->aci = cmd->tone_antenna_config_selection;
+ params->phy = cmd->phy;
+ params->tx_power_delta = cmd->tx_power_delta;
+ params->preferred_peer_antenna = cmd->preferred_peer_antenna;
+ params->snr_control_initiator = cmd->snr_control_initiator;
+ params->snr_control_reflector = cmd->snr_control_reflector;
+ params->params_ready = 1;
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
 }
 
 int

nimble/controller/src/ble_ll_cs_priv.h

@@ -55,6 +55,22 @@ struct ble_ll_cs_supp_cap {
  uint8_t tx_snr_capablity;
 };
 
+struct ble_ll_cs_pref_proc_params {
+ uint16_t max_procedure_len;
+ uint16_t min_procedure_interval;
+ uint16_t max_procedure_interval;
+ uint16_t max_procedure_count;
+ uint32_t min_subevent_len;
+ uint32_t max_subevent_len;
+ uint8_t aci;
+ uint8_t phy;
+ uint8_t tx_power_delta;
+ uint8_t preferred_peer_antenna;
+ uint8_t snr_control_initiator;
+ uint8_t snr_control_reflector;
+ uint8_t params_ready;
+};
+
 struct ble_ll_cs_config {
  bool config_enabled;
  /* The role to use in CS procedure
@@ -88,6 +104,8 @@ struct ble_ll_cs_config {
  uint8_t t_ip2;
  uint8_t t_fcs;
  uint8_t t_pm;
+ /* CS procedure parameters preferred by our Host */
+ struct ble_ll_cs_pref_proc_params pref_proc_params;
 };
 
 struct ble_ll_cs_sm {
@@ -110,6 +128,8 @@ struct ble_ll_cs_sm {
 
  /* DRBG context, initialized onece per LE Connection */
  struct ble_ll_cs_drbg_ctx drbg_ctx;
+
+ uint8_t measurement_enabled;
 };
 
 #ifdef __cplusplus

nimble/include/nimble/ble.h

@@ -264,6 +264,9 @@ enum ble_error_codes
  BLE_ERR_LIMIT_REACHED = 0x43,
  BLE_ERR_OPERATION_CANCELLED = 0x44,
  BLE_ERR_PACKET_TOO_LONG = 0x45,
+ BLE_ERR_TOO_LATE = 0x46,
+ BLE_ERR_TOO_EARLY = 0x47,
+ BLE_ERR_INSUFFICIENT_CHAN = 0x48,
  BLE_ERR_MAX = 0xff
 };