C++ cfw_send_message函数代码示例

void sensor_service_set_calibration(cfw_service_conn_t *p_service_conn,
				    void *p_priv,
				    sensor_service_t sensor,
				    uint8_t calibration_type, uint8_t len,
				    uint8_t *value)
{
	ss_sensor_calibration_req_t *p_msg;

	/* Allocate sensor setting calibration request message */
	p_msg = (ss_sensor_calibration_req_t *)cfw_alloc_message_for_service(
		p_service_conn,
		MSG_ID_SS_SENSOR_CALIBRATION_REQ,
		sizeof(*p_msg) + len, p_priv);
	if (p_msg == NULL) {
		return;
	}
	p_msg->sensor = sensor;
	p_msg->calibration_type = calibration_type;
	p_msg->clb_cmd = SET_CALIBRATION_CMD;
	p_msg->data_length = len;
	/* Fill cal param*/
	memcpy(p_msg->value, value, len);

	/* Send the message */
	cfw_send_message(p_msg);
}

/*******************************************************************************
 *********************** SERVICE IMPLEMENTATION ********************************
 ******************************************************************************/
void handle_get(struct cfw_message *msg)
{
	circular_storage_get_req_msg_t *req =
		(circular_storage_get_req_msg_t *)msg;
	circular_storage_service_get_rsp_msg_t *resp =
		(circular_storage_service_get_rsp_msg_t *)cfw_alloc_rsp_msg(
			msg,
			MSG_ID_CIRCULAR_STORAGE_SERVICE_GET_RSP,
			sizeof(*resp));
	DRIVER_API_RC ret = DRV_RC_FAIL;
	uint32_t i = 0;

	for (i = 0; i < cir_storage_config.cir_storage_count; i++) {
		if (cir_storage_config.cir_storage_list[i].key == req->key) {
			if (cir_storage_config.cir_storage_list[i].storage !=
			    NULL) {
				ret = DRV_RC_OK;
				resp->storage =
					cir_storage_config.cir_storage_list[i].
					storage;
			} else {
				pr_debug(
					LOG_MODULE_MAIN,
					"Circular storage init: Flash SPI init failed");
				ret = DRV_RC_FAIL;
			}
		}
	}

	resp->status = ret;
	cfw_send_message(resp);
}

int ble_gatts_add_descriptor(svc_client_handle_t * p_svc_handle,
			     const struct ble_gatts_descriptor * p_desc,
			     ble_gatts_add_desc_cback_t cback,
			     void * p_priv)
{
	struct ble_gatts_add_desc_req_msg *req;
	uint8_t * p;
	int data_len = sizeof(struct ble_gatts_add_desc_req_msg);

	/* Sanity check */
	if (!p_desc || !p_desc->length || !p_desc->p_value || !p_desc->p_uuid)
		return E_OS_ERR;

	// compute the variable length part of the message
	data_len += ble_sizeof_bt_uuid(p_desc->p_uuid);
	data_len += p_desc->length;

	req = (struct ble_gatts_add_desc_req_msg *)
	    cfw_alloc_message_for_service(p_svc_handle,
			    MSG_ID_BLE_GATTS_ADD_DESCRIPTOR_REQ,
			    data_len, p_priv);
	req->cback = cback;
	p = req->data;
	p = ble_encode_bt_uuid(p_desc->p_uuid, p);
	req->length = p_desc->length;
	req->perms = p_desc->perms;
	memcpy(p, p_desc->p_value, p_desc->length);

	return cfw_send_message(req);
}

static void handle_message(struct cfw_message *msg, void *param)
{
	switch (CFW_MESSAGE_ID(msg)) {
	case MSG_ID_CIRCULAR_STORAGE_PUSH_REQ:
		handle_push(msg);
		break;
	case MSG_ID_CIRCULAR_STORAGE_POP_REQ:
		handle_pop(msg);
		break;
	case MSG_ID_CIRCULAR_STORAGE_PEEK_REQ:
		handle_peek(msg);
		break;
	case MSG_ID_CIRCULAR_STORAGE_CLEAR_REQ:
		handle_clear(msg);
		break;
	case MSG_ID_LL_CIRCULAR_STORAGE_SHUTDOWN_REQ:
		cfw_send_message(CFW_MESSAGE_PRIV(msg));
		break;
	case MSG_ID_CIRCULAR_STORAGE_GET_REQ:
		handle_get(msg);
	default:
		cfw_print_default_handle_error_msg(LOG_MODULE_MAIN,
						   CFW_MESSAGE_ID(
							   msg));
		break;
	}

	cfw_msg_free(msg);
}

int ss_sensor_subscribe_data(cfw_service_conn_t *p_service_conn, void *p_priv, ss_sensor_t sensor,
                             uint8_t *data_type, uint8_t data_type_nr, uint16_t sampling_interval,
                             uint16_t reporting_interval)
{
    ss_sensor_subscribe_data_req_t *p_msg;

    if (data_type_nr == 0) {
        return -1;
    }
    /* Allocate sensor subscribing data request message */
    p_msg = (ss_sensor_subscribe_data_req_t *) cfw_alloc_message_for_service(p_service_conn,
                    MSG_ID_SS_SENSOR_SUBSCRIBE_DATA_REQ, (sizeof(*p_msg) + (data_type_nr - 1) *
                                                          sizeof(uint8_t)), p_priv);
    if (p_msg == NULL) {
        return -1;
    }
    /* Fill Request Parameter */
    p_msg->sensor = sensor;
    p_msg->data_type_nr = data_type_nr;
    p_msg->sampling_interval = sampling_interval;
    p_msg->reporting_interval = reporting_interval;

    /* Fill Request Parameter */
    memcpy(p_msg->data_type ,data_type ,sizeof(uint8_t) * data_type_nr);

    /* Send the message */
    return cfw_send_message(p_msg);
}

/**@brief Callback to manage results of requests of setting the battery level alarm threshold.
 * @param[in]  msg    Request message.
 *             msg_id Type of level alarm threshold.
 */
static void handle_bs_set_level_alarm(struct cfw_message *msg, int msg_id)
{
	int rsp_id = 0;
	int level_alarm_threshold = 0;

	fg_status_t (*fg_set_alarm)(uint8_t) = NULL;
	battery_set_level_alarm_rsp_msg_t *resp;

	switch (msg_id) {
	case MSG_ID_BATTERY_SERVICE_SET_LOW_LEVEL_ALARM_REQ:
		rsp_id = MSG_ID_BATTERY_SERVICE_SET_LOW_LEVEL_ALARM_RSP;
		fg_set_alarm = &fg_set_low_level_alarm_threshold;
		break;
	case MSG_ID_BATTERY_SERVICE_SET_CRITICAL_LEVEL_ALARM_REQ:
		rsp_id = MSG_ID_BATTERY_SERVICE_SET_CRITICAL_LEVEL_ALARM_RSP;
		fg_set_alarm = &fg_set_critical_level_alarm_threshold;
		break;
	default:
		assert(0);
		break;
	}
	level_alarm_threshold =
		((battery_set_level_alarm_msg_t *)msg)->level_alarm;
	resp = (battery_set_level_alarm_rsp_msg_t *)cfw_alloc_rsp_msg(
		msg, rsp_id, sizeof(*resp));
	resp->status = BATTERY_STATUS_ERROR_FUEL_GAUGE;
	if (FG_STATUS_SUCCESS == fg_set_alarm(level_alarm_threshold))
		resp->status = BATTERY_STATUS_SUCCESS;
	cfw_send_message(resp);

	return;
}

/**@brief Callback to manage results of requests of setting ADC measure interval
 * @param[in]  msg  Request message.
 */
static void handle_battery_set_measure_interval(struct cfw_message *msg)
{
	uint16_t new_period_ms = 0;
	battery_cfg_type_t cfg_period;

	struct cfw_message *resp;

	resp = cfw_alloc_rsp_msg(
		msg,
		MSG_ID_BATTERY_SERVICE_SET_MEASURE_INTERVAL_RSP,
		sizeof(*resp));
	new_period_ms =
		((battery_set_measure_interval_msg_t *)msg)->period_cfg.
		new_period_ms;
	cfg_period =
		((battery_set_measure_interval_msg_t *)msg)->period_cfg.
		cfg_type;
	if (cfg_period == CFG_TEMPERATURE) {
		fg_set_temp_interval(new_period_ms);
	} else if (cfg_period == CFG_VOLTAGE) {
		fg_set_voltage_interval(new_period_ms);
	}
	cfw_send_message(resp);

	return;
}

static int handle_ble_init_service_bas(struct ble_init_svc_req *msg,
				struct _ble_service_cb *p_cb)
{
	struct ble_init_service_rsp *resp;
	struct _bt_gatt_ccc *ccc = bas_attrs[BLVL_CCC_HANDLE_IDX].user_data;
	int status;

	/* use CCCD value for ble service specific purpose to save ram.
	 * notif & indication use the 2 LSBs. Allow battery updated by default
	 */
	ccc->value = BLE_SVC_BAS_NOTIF_SUP_ON;

	status = bt_gatt_register((struct bt_gatt_attr *)bas_attrs,
				ARRAY_SIZE(bas_attrs));

	resp = ble_alloc_init_service_rsp(msg);
	if (status)
		resp->status = BLE_STATUS_ERROR;

	cfw_send_message(resp);

#ifdef CONFIG_SERVICES_BLE_BAS_USE_BAT
	cfw_register_svc_available(_ble_cb.client, BATTERY_SERVICE_ID, &_ble_cb);
#endif
	return 0;
}

/**@brief Callback to manage results of requests for getting the battery information.
 * @param[in]  msg  Request message.
 */
static void handle_battery_service_get_info(struct cfw_message *msg)
{
	battery_service_info_request_rsp_msg_t *resp =
		(battery_service_info_request_rsp_msg_t *)cfw_alloc_rsp_msg(
			msg,
			MSG_ID_BATTERY_SERVICE_GET_BATTERY_INFO_RSP,
			sizeof(
				*resp));

	switch (((battery_status_msg_t *)msg)->batt_info_id) {
	case BATTERY_DATA_LEVEL:
		handle_bs_get_soc(&resp->battery_soc);
		resp->status = resp->battery_soc.status;
		break;
	case BATTERY_DATA_STATUS:
		resp->battery_charging.is_charging = charging_sm_is_charging();
		resp->status = resp->battery_charging.status =
				       BATTERY_STATUS_SUCCESS;
		break;
	case BATTERY_DATA_VBATT:
		handle_bs_get_voltage(&resp->battery_voltage);
		resp->status = resp->battery_voltage.status;
		break;
	case BATTERY_DATA_TEMPERATURE:
		handle_bs_get_temperature(&resp->battery_temperature);
		resp->status = resp->battery_temperature.status;
		break;
	case BATTERY_DATA_CHARGE_CYCLE:
		handle_bs_get_charge_cycle(&resp->battery_charge_cycle);
		resp->status = resp->battery_charge_cycle.status;
		break;
	case BATTERY_DATA_CHARGER_STATUS:
		resp->battery_charger_connected.is_charger_connected =
			charging_sm_is_charger_connected();
		resp->status = resp->battery_charger_connected.status =
				       BATTERY_STATUS_SUCCESS;
		break;
	case BATTERY_DATA_CHARGER_TYPE:
		resp->battery_charging_source.charging_source =
			charging_sm_get_source();
		resp->status = resp->battery_charging_source.status =
				       BATTERY_STATUS_SUCCESS;
		break;
	case BATTERY_DATA_LOW_LEVEL_ALARM:
		handle_bs_get_low_level_alarm(&resp->battery_low_level_alarm);
		resp->status = resp->battery_low_level_alarm.status;
		break;
	case BATTERY_DATA_CRITICAL_LEVEL_ALARM:
		handle_bs_get_critical_level_alarm(
			&resp->battery_critical_level_alarm);
		resp->status = resp->battery_critical_level_alarm.status;
		break;
	default:
		break;
	}

	resp->batt_info_id = ((battery_status_msg_t *)msg)->batt_info_id;
	cfw_send_message(resp);
	return;
}

int ss_send_sensor_cmd(cfw_service_conn_t *p_service_conn, void *p_priv,
		       sensor_service_t sensor, uint32_t cmd_id,
		       cmd_parameter_t parameter)
{
	ss_send_sensor_cmd_req_t *p_msg;

	int msg_size = parameter.length + sizeof(*p_msg) +
		       sizeof(parameter.length);

	/* Allocate sending sensor cmd request message */
	p_msg = (ss_send_sensor_cmd_req_t *)cfw_alloc_message_for_service(
		p_service_conn,
		MSG_ID_SS_SEND_SENSOR_CMD_REQ,
		msg_size, p_priv);
	if (p_msg == NULL) {
		return -1;
	}
	/* Fill Request Parameter */
	p_msg->sensor = sensor;
	p_msg->cmd_id = cmd_id;
	p_msg->parameter = parameter;

	/* Send the message */
	return cfw_send_message(p_msg);
}

void sensor_service_unsubscribe_data(cfw_service_conn_t *	p_service_conn,
				     void *			p_priv,
				     sensor_service_t		sensor,
				     uint8_t *			data_type,
				     uint8_t			data_type_nr)
{
	ss_sensor_unsubscribe_data_req_t *p_msg;


	if (data_type_nr == 0) {
		return;
	}
	/* Allocate sensor unsubscribing data request message */
	p_msg =
		(ss_sensor_unsubscribe_data_req_t *)
		cfw_alloc_message_for_service(
			p_service_conn,
			MSG_ID_SS_SENSOR_UNSUBSCRIBE_DATA_REQ,
			sizeof(*p_msg) + (data_type_nr - 1) * sizeof(uint8_t),
			p_priv);
	if (p_msg == NULL) {
		return;
	}
	/* Fill Request Parameter */
	p_msg->sensor = sensor;
	p_msg->data_type_nr = data_type_nr;
	/* Fill Request Parammeter */
	memcpy(p_msg->data_type, data_type, sizeof(uint8_t) * data_type_nr);

	/* Send the message */
	cfw_send_message(p_msg);
}

void handle_erase_block(struct cfw_message *msg)
{
    ll_storage_erase_block_req_msg_t * req = (ll_storage_erase_block_req_msg_t *) msg;
    ll_storage_erase_block_rsp_msg_t * resp = (ll_storage_erase_block_rsp_msg_t *) cfw_alloc_rsp_msg(msg,
            MSG_ID_LL_ERASE_BLOCK_RSP, sizeof(*resp));

    flash_device_t flash;
    uint16_t flash_id = 0;
    int16_t partition_index = -1;
    uint32_t i = 0;
    DRIVER_API_RC ret = DRV_RC_FAIL;

    if (req->no_blks == 0) {
        pr_debug(LOG_MODULE_MAIN, "LL Storage Service - Write Block: Invalid write size");
        ret = DRV_RC_INVALID_OPERATION;
        goto send;
    }

    for (i = 0; i < ll_storage_config.no_part; i++)
        if (ll_storage_config.partitions[i].partition_id == req->partition_id) {
            flash_id = ll_storage_config.partitions[i].flash_id;
            partition_index = i;
            break;
        }

    if (partition_index == -1) {
        pr_debug(LOG_MODULE_MAIN, "LL Storage Service - Write Block: Invalid partition ID");
        ret = DRV_RC_FAIL;
        goto send;
    }

    uint16_t last_block = ll_storage_config.partitions[partition_index].start_block + req->st_blk + req->no_blks - 1;

    if (last_block > ll_storage_config.partitions[partition_index].end_block) {
        pr_debug(LOG_MODULE_MAIN, "LL Storage Service - Write Block: Partition overflow");
        ret = DRV_RC_OUT_OF_MEM;
        goto send;
    }

    flash = flash_devices[flash_id];

    if (flash.flash_location == EMBEDDED_FLASH)
    {
        ret = soc_flash_block_erase(ll_storage_config.partitions[partition_index].start_block + req->st_blk, req->no_blks);
    }
#ifdef CONFIG_SPI_FLASH
    else
    {
        // SERIAL_FLASH
        ret = spi_flash_sector_erase((struct device *)&pf_sba_device_flash_spi0,
            ll_storage_config.partitions[partition_index].start_block + req->st_blk, req->no_blks);
    }
#endif

send:
    resp->rsp_header.status = ret;
    cfw_send_message(resp);
}

void on_nble_common_rsp(const struct nble_response *params)
{
	struct ble_rsp *resp = params->user_data;

	if (!resp)
		return;
	resp->status = params->status;
	cfw_send_message(resp);
}

static void bat_lvl_notified(struct bt_conn *conn, uint16_t handle, uint8_t err)
{
	if (cli_rsp) {
		cli_rsp->status = (err == 0) ? BLE_STATUS_SUCCESS :
				BLE_STATUS_ERROR;
		cfw_send_message(cli_rsp);
		cli_rsp = NULL;
	}
}

int ble_init_service_bas(cfw_service_conn_t * p_service_conn, void *priv)
{
	CFW_ALLOC_FOR_SVC(struct ble_init_svc_req, msg, p_service_conn,
			  MSG_ID_BLE_INIT_SVC_REQ, 0, priv);

	msg->init_svc = handle_ble_init_service_bas;

	return cfw_send_message(msg);
}