/**
* @brief Request meta-data from camera module
*
* Allows the Camera to provide meta-data associated with a frame to the AP over
* Greybus.
*
* @param operation pointer to structure of Greybus operation message
* @return GB_OP_SUCCESS on success, error code on failure
*/
static uint8_t gb_camera_metadata(struct gb_operation *operation)
{
struct gb_camera_meta_data_request *request;
struct metadata_info meta_data;
int ret;
lldbg("gb_camera_metadata() + \n");
if (gb_operation_get_request_payload_size(operation) < sizeof(*request)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
request = gb_operation_get_request_payload(operation);
meta_data.request_id = le32_to_cpu(request->request_id);
meta_data.frame_number = le16_to_cpu(request->frame_number);
meta_data.stream = request->stream;
meta_data.padding = request->padding;
meta_data.data = request->data;
lldbg(" request_id = %d \n", request->request_id);
lldbg(" frame_number = %d \n", request->frame_number);
lldbg(" stream = %d \n", request->stream);
ret = device_camera_trans_metadata(info->dev, &meta_data);
if (ret) {
return gb_errno_to_op_result(ret);
}
lldbg("gb_camera_metadata() - \n");
return GB_OP_SUCCESS;
}
/**
* @brief Get Camera capabilities
*
* This operation retrieves the list of capabilities of the Camera Module and
* then returns to host.
*
* @param operation Pointer to structure of Greybus operation.
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_camera_capabilities(struct gb_operation *operation)
{
struct gb_camera_capabilities_response *response;
const uint8_t *caps;
size_t size;
int ret;
lldbg("gb_camera_capabilities() + \n");
if (info->state < STATE_UNCONFIGURED) {
lldbg("state error %d \n", info->state);
return GB_OP_INVALID;
}
/* Retrieve the capabilities and their size. */
ret = device_camera_capabilities(info->dev, &size, &caps);
if (ret) {
return gb_errno_to_op_result(ret);
}
if (size > GB_MAX_PAYLOAD_SIZE) {
return GB_OP_NO_MEMORY;
}
response = gb_operation_alloc_response(operation, sizeof(*response) + size);
if (!response) {
return GB_OP_NO_MEMORY;
}
memcpy(response->capabilities, caps, size);
lldbg("gb_camera_capabilities() - \n");
return GB_OP_SUCCESS;
}
static uint8_t gb_control_connected(struct gb_operation *operation)
{
int retval;
struct gb_control_connected_request *request =
gb_operation_get_request_payload(operation);
if (gb_operation_get_request_payload_size(operation) < sizeof(*request)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
retval = gb_listen(le16_to_cpu(request->cport_id));
if (retval) {
gb_error("Can not connect cport %d: error %d\n",
le16_to_cpu(request->cport_id), retval);
return GB_OP_INVALID;
}
retval = gb_notify(le16_to_cpu(request->cport_id), GB_EVT_CONNECTED);
if (retval)
goto error_notify;
return GB_OP_SUCCESS;
error_notify:
gb_stop_listening(le16_to_cpu(request->cport_id));
return gb_errno_to_op_result(retval);
}
static uint8_t gb_svc_dme_peer_set(struct gb_operation *op) {
struct gb_svc_dme_peer_set_request *req;
struct gb_svc_dme_peer_set_response *resp;
int rc;
uint16_t attr, selector;
uint32_t value;
if (gb_operation_get_request_payload_size(op) < sizeof(*req)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
req = gb_operation_get_request_payload(op);
resp = gb_operation_alloc_response(op, sizeof(*resp));
if (!resp) {
return GB_OP_NO_MEMORY;
}
attr = le16_to_cpu(req->attr);
selector = le16_to_cpu(req->selector);
value = le32_to_cpu(req->value);
rc = svc_dme_peer_set(req->intf_id, attr, selector, value,
&resp->result_code);
resp->result_code = cpu_to_le16(resp->result_code);
return gb_errno_to_op_result(rc);
}
static uint8_t gb_control_timesync_disable(struct gb_operation *operation)
{
int retval;
retval = timesync_disable();
return gb_errno_to_op_result(retval);
}
/**
* @brief Flush the camera capture
*
* The Flush operation calls camera driver to flush capture.
*
* @param operation pointer to structure of Greybus operation message
* @return GB_OP_SUCCESS on success, error code on failure
*/
static uint8_t gb_camera_flush(struct gb_operation *operation)
{
struct gb_camera_flush_response *response;
uint32_t request_id = 0;
int ret;
lldbg("gb_camera_flush() + \n");
if (info->state != STATE_STREAMING && info->state != STATE_CONFIGURED) {
return GB_OP_INVALID;
}
ret = device_camera_flush(info->dev, &request_id);
if (ret) {
return gb_errno_to_op_result(ret);
}
info->state = STATE_CONFIGURED;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response) {
return GB_OP_NO_MEMORY;
}
response->request_id = cpu_to_le32(request_id);
lldbg(" request_id = %d + \n", request_id);
lldbg("gb_camera_flush() + \n");
return GB_OP_SUCCESS;
}
/**
* @brief Engage camera capture operation
*
* It tell camera module to start capture.
*
* @param operation pointer to structure of Greybus operation message
* @return GB_OP_SUCCESS on success, error code on failure
*/
static uint8_t gb_camera_capture(struct gb_operation *operation)
{
struct gb_camera_capture_request *request;
struct capture_info *capt_req;
size_t request_size;
int ret;
lldbg("gb_camera_capture() + \n");
if (info->state != STATE_CONFIGURED && info->state != STATE_STREAMING) {
return GB_OP_INVALID;
}
request_size = gb_operation_get_request_payload_size(operation);
if (request_size < sizeof(*request)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
request = gb_operation_get_request_payload(operation);
if (request->padding != 0) {
gb_error("invalid padding value\n");
return GB_OP_INVALID;
}
capt_req = malloc(sizeof(*capt_req));
if(!capt_req) {
return GB_OP_NO_MEMORY;
}
capt_req->request_id = le32_to_cpu(request->request_id);
capt_req->streams = request->streams;
capt_req->num_frames = le32_to_cpu(request->num_frames);
capt_req->settings = request->settings;
capt_req->settings_size = request_size - sizeof(*request);
lldbg(" request_id = %d \n", capt_req->request_id);
lldbg(" streams = %d \n", capt_req->streams);
lldbg(" num_frames = %d \n", capt_req->num_frames);
lldbg(" settings_size = %u\n", capt_req->settings_size);
ret = device_camera_capture(info->dev, capt_req);
if (ret) {
gb_error("error in camera capture thread. \n");
ret = gb_errno_to_op_result(ret);
goto err_free_mem;
}
free(capt_req);
lldbg("gb_camera_capture() - \n");
return GB_OP_SUCCESS;
err_free_mem:
free(capt_req);
return ret;
}
static uint8_t gb_svc_route_destroy(struct gb_operation *op) {
struct gb_svc_route_destroy_request *req;
int rc;
if (gb_operation_get_request_payload_size(op) < sizeof(*req)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
req = gb_operation_get_request_payload(op);
rc = svc_route_destroy(req->intf1_id, req->intf2_id);
return gb_errno_to_op_result(rc);
}
/**
* @brief Get Camera capabilities
*
* This operation retrieves the list of capabilities of the Camera Module and
* then returns to host.
*
* @param operation Pointer to structure of Greybus operation.
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_camera_capabilities(struct gb_operation *operation)
{
struct gb_camera_capabilities_response *response;
uint8_t *capabilities;
uint16_t size;
int ret;
lldbg("gb_camera_capabilities() + \n");
if (info->state < STATE_UNCONFIGURED) {
lldbg("state error %d \n", info->state);
return GB_OP_INVALID;
}
ret = device_camera_get_required_size(info->dev, SIZE_CAPABILITIES, &size);
if (ret) {
return gb_errno_to_op_result(ret);
}
response = gb_operation_alloc_response(operation, sizeof(*response) + size);
if (!response) {
return GB_OP_NO_MEMORY;
}
/* camera module capabilities */
ret = device_camera_capabilities(info->dev, &size, capabilities);
if (ret) {
return gb_errno_to_op_result(ret);
}
response->size = cpu_to_le16(size);
memcpy(response->capabilities, &capabilities, size);
lldbg("gb_camera_capabilities() - \n");
return GB_OP_SUCCESS;
}
static uint8_t gb_svc_connection_destroy(struct gb_operation *op)
{
int retval;
struct gb_svc_conn_destroy_request *req;
if (gb_operation_get_request_payload_size(op) < sizeof(*req)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
req = gb_operation_get_request_payload(op);
retval = svc_connection_destroy(req->intf1_id, le16_to_cpu(req->cport1_id),
req->intf2_id, le16_to_cpu(req->cport2_id));
return gb_errno_to_op_result(retval);
}
static uint8_t gb_svc_connection_create(struct gb_operation *op) {
struct gb_svc_conn_create_request *req;
int rc;
if (gb_operation_get_request_payload_size(op) < sizeof(*req)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
req = gb_operation_get_request_payload(op);
rc = svc_connection_create(req->intf1_id, le16_to_cpu(req->cport1_id),
req->intf2_id, le16_to_cpu(req->cport2_id),
req->tc, req->flags);
return gb_errno_to_op_result(rc);
}
/**
* @brief Get battery total capacity in mAh.
*
* @param operation The pointer to structure of gb_operation.
*
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_battery_capacity(struct gb_operation *operation)
{
struct gb_battery_capacity_response *response;
uint32_t capacity = 0;
int ret = 0;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response)
return GB_OP_NO_MEMORY;
ret = device_battery_total_capacity(batt_dev, &capacity);
response->capacity = cpu_to_le32(capacity);
return gb_errno_to_op_result(ret);
}
/**
* @brief Get battery current in microampere.
*
* @param operation The pointer to structure of gb_operation.
*
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_battery_current(struct gb_operation *operation)
{
struct gb_battery_current_response *response;
int current = 0;
int ret = 0;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response)
return GB_OP_NO_MEMORY;
ret = device_battery_current(batt_dev, ¤t);
response->current = cpu_to_le32(current);
return gb_errno_to_op_result(ret);
}
/**
* @brief Get battery current voltage in microvolt .
*
* @param operation The pointer to structure of gb_operation.
*
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_battery_voltage(struct gb_operation *operation)
{
struct gb_battery_voltage_response *response;
uint32_t voltage = 0;
int ret = 0;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response)
return GB_OP_NO_MEMORY;
ret = device_battery_voltage(batt_dev, &voltage);
response->voltage = cpu_to_le32(voltage);
return gb_errno_to_op_result(ret);
}
/**
* @brief Get battery status.
*
* @param operation The pointer to structure of gb_operation.
*
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_battery_status(struct gb_operation *operation)
{
struct gb_battery_status_response *response;
uint16_t status = 0;
int ret = 0;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response)
return GB_OP_NO_MEMORY;
ret = device_battery_status(batt_dev, &status);
response->status = cpu_to_le16(status);
return gb_errno_to_op_result(ret);
}
static uint8_t gb_control_timesync_get_last_event(
struct gb_operation *operation)
{
uint64_t frame_time;
struct gb_control_timesync_get_last_event_response *response;
int retval;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response)
return GB_OP_NO_MEMORY;
retval = timesync_get_last_event(&frame_time);
if (!retval)
response->frame_time = cpu_to_le64(frame_time);
return gb_errno_to_op_result(retval);
}
/**
* @brief Get the battery adapter control technology type.
*
* @param operation The pointer to structure of gb_operation.
*
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_battery_technology(struct gb_operation *operation)
{
struct gb_battery_technology_response *response;
uint32_t tech = 0;
int ret = 0;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response)
return GB_OP_NO_MEMORY;
ret = device_battery_technology(batt_dev, &tech);
response->technology = cpu_to_le32(tech);
return gb_errno_to_op_result(ret);
}
/**
* @brief Get battery shutdown temperature in 0.1 Celsius.
*
* @param operation The pointer to structure of gb_operation.
*
* @return GB_OP_SUCCESS on success, error code on failure.
*/
static uint8_t gb_battery_shutdown_temp(struct gb_operation *operation)
{
struct gb_battery_shutdown_temperature_response *response;
int temp = 0;
int ret = 0;
response = gb_operation_alloc_response(operation, sizeof(*response));
if (!response)
return GB_OP_NO_MEMORY;
ret = device_battery_shutdown_temp(batt_dev, &temp);
response->temperature = cpu_to_le32(temp);
return gb_errno_to_op_result(ret);
}
/*
* SVC Protocol Request handlers
*/
static uint8_t gb_svc_intf_device_id(struct gb_operation *op) {
struct gb_svc_intf_device_id_request *req;
u8 intf_id;
u8 dev_id;
int rc;
if (gb_operation_get_request_payload_size(op) < sizeof(*req)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
req = gb_operation_get_request_payload(op);
intf_id = req->intf_id;
dev_id = req->device_id;
rc = svc_intf_device_id(intf_id, dev_id);
return gb_errno_to_op_result(rc);
}
static uint8_t gb_control_timesync_authoritative(struct gb_operation *operation)
{
uint64_t frame_time[GB_TIMESYNC_MAX_STROBES];
struct gb_control_timesync_authoritative_request *request;
int i;
int retval;
if (gb_operation_get_request_payload_size(operation) < sizeof(*request)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
request = gb_operation_get_request_payload(operation);
for (i = 0; i < GB_TIMESYNC_MAX_STROBES; i++)
frame_time[i] = le64_to_cpu(request->frame_time[i]);
retval = timesync_authoritative(frame_time);
return gb_errno_to_op_result(retval);
}
static uint8_t gb_control_timesync_enable(struct gb_operation *operation)
{
uint8_t count;
uint64_t frame_time;
uint32_t strobe_delay;
uint32_t refclk;
struct gb_control_timesync_enable_request *request;
int retval;
if (gb_operation_get_request_payload_size(operation) < sizeof(*request)) {
gb_error("dropping short message\n");
return GB_OP_INVALID;
}
request = gb_operation_get_request_payload(operation);
count = request->count;
frame_time = le64_to_cpu(request->frame_time);
strobe_delay = le32_to_cpu(request->strobe_delay);
refclk = le32_to_cpu(request->refclk);
retval = timesync_enable(count, frame_time, strobe_delay, refclk);
return gb_errno_to_op_result(retval);
}
/**
* @brief Configure camera module streams
*
* The Configure Streams operation configures or unconfigures the Camera Module
* to prepare or stop video capture.
*
* @param operation pointer to structure of Greybus operation message
* @return GB_OP_SUCCESS on success, error code on failure
*/
static uint8_t gb_camera_configure_streams(struct gb_operation *operation)
{
struct gb_camera_configure_streams_request *request;
struct gb_camera_configure_streams_response *response;
struct gb_stream_config_req *cfg_set_req;
struct gb_stream_config_resp *cfg_ans_resp;
struct streams_cfg_req *cfg_request;
struct streams_cfg_ans *cfg_answer;
uint8_t num_streams;
uint8_t res_flags = 0;
int i, ret;
lldbg("gb_camera_configure_streams() + \n");
if (gb_operation_get_request_payload_size(operation) < sizeof(*request)) {
gb_error("dropping short message \n");
return GB_OP_INVALID;
}
request = gb_operation_get_request_payload(operation);
num_streams = request->num_streams;
lldbg("num_streams = %d \n", num_streams);
lldbg("req flags = %d \n", request->flags);
if (num_streams > MAX_STREAMS_NUM)
return GB_OP_INVALID;
/* Check if the request is acceptable in the current state. */
if (num_streams == 0) {
if (info->state < STATE_UNCONFIGURED || info->state > STATE_CONFIGURED)
return GB_OP_INVALID;
} else {
if (info->state != STATE_UNCONFIGURED)
return GB_OP_INVALID;
}
/*
* Zero streams unconfigures the camera, move to the unconfigured state and
* power it down.
*/
if (num_streams == 0) {
info->state = STATE_UNCONFIGURED;
ret = device_camera_power_down(info->dev);
if (ret)
return gb_errno_to_op_result(ret);
response = gb_operation_alloc_response(operation, sizeof(*response));
return GB_OP_SUCCESS;
}
/* Otherwise pass stream configuration to the camera module. */
cfg_set_req = request->config;
cfg_request = malloc(num_streams * sizeof(*cfg_request));
if (!cfg_request)
return GB_OP_NO_MEMORY;
/* convert data for driver */
for (i = 0; i < num_streams; i++) {
lldbg(" stream #%d\n", i);
cfg_request[i].width = le16_to_cpu(cfg_set_req[i].width);
cfg_request[i].height = le16_to_cpu(cfg_set_req[i].height);
cfg_request[i].format = le16_to_cpu(cfg_set_req[i].format);
cfg_request[i].padding = le16_to_cpu(cfg_set_req[i].padding);
lldbg(" width = %d \n", cfg_request[i].width);
lldbg(" height = %d \n", cfg_request[i].height);
lldbg(" format = %d \n", cfg_request[i].format);
lldbg(" padding = %d \n", cfg_request[i].padding);
}
/* alloc for getting answer from driver */
cfg_answer = malloc(MAX_STREAMS_NUM * sizeof(*cfg_answer));
if (!cfg_answer) {
ret = GB_OP_NO_MEMORY;
goto err_free_req_mem;
}
/* driver shall check the num_streams, it can't exceed its capability */
ret = device_camera_set_streams_cfg(info->dev, &num_streams,
request->flags, cfg_request,
&res_flags, cfg_answer);
if (ret) {
/* FIXME:
* add greybus protocol error for EIO operations.
* For now, return OP_INVALID
*/
lldbg("Camera module reported error in configure stream %d\n", ret);
ret = GB_OP_INVALID;
goto err_free_ans_mem;
}
/*
* If the requested format is not supported keep camera in un-configured
* state;
* Stay un-configured anyhow if AP is just testing format;
* Move to configured otherwise
*/
if (res_flags & CAMERA_CONF_STREAMS_ADJUSTED)
info->state = STATE_UNCONFIGURED;
else if (request->flags & CAMERA_CONF_STREAMS_TEST_ONLY)
info->state = STATE_UNCONFIGURED;
else
info->state = STATE_CONFIGURED;
/* Create and fill the greybus response. */
lldbg("Resp: \n");
response = gb_operation_alloc_response(operation,
sizeof(*response) + num_streams * sizeof(*cfg_ans_resp));
response->num_streams = num_streams;
response->flags = res_flags;
response->padding[0] = 0;
response->padding[1] = 0;
lldbg("flags = 0x%2x: \n", response->flags);
for (i = 0; i < num_streams; i++) {
cfg_ans_resp = &response->config[i];
lldbg("\n");
lldbg(" width = %d \n", cfg_answer[i].width);
lldbg(" height = %d \n", cfg_answer[i].height);
lldbg(" format = %d \n", cfg_answer[i].format);
lldbg(" virtual_channel = %d \n", cfg_answer[i].virtual_channel);
lldbg(" data_type = %d \n", cfg_answer[i].data_type);
lldbg(" max_size = %d \n", cfg_answer[i].max_size);
cfg_ans_resp->width = cpu_to_le16(cfg_answer[i].width);
cfg_ans_resp->height = cpu_to_le16(cfg_answer[i].height);
cfg_ans_resp->format = cpu_to_le16(cfg_answer[i].format);
cfg_ans_resp->virtual_channel = cfg_answer[i].virtual_channel;
/*
* FIXME
* The API towards the camera driver supports a single data type
* for now, always return NOT_USED for the second data type
*/
cfg_ans_resp->data_type[0] = cfg_answer[i].data_type;
cfg_ans_resp->data_type[1] = GB_CAM_DT_NOT_USED;
cfg_ans_resp->padding[0] = 0;
cfg_ans_resp->padding[1] = 0;
cfg_ans_resp->padding[2] = 0;
cfg_ans_resp->max_size = cpu_to_le32(cfg_answer[i].max_size);
}
ret = GB_OP_SUCCESS;
err_free_ans_mem:
free(cfg_answer);
err_free_req_mem:
free(cfg_request);
lldbg("gb_camera_configure_streams() %d - \n", ret);
return ret;
}