/*
* Validate the given descriptor. Its reported size must fit within
* the number of bytes reamining, and it must have a recognized
* type. Check that the reported size is at least as big as what
* we expect to see. (It could be bigger, perhaps for a new version
* of the format.)
*
* Returns the number of bytes consumed by the descriptor, or a
* negative errno.
*/
static int identify_descriptor(struct gbsim_interface *intf,
struct greybus_descriptor *desc, size_t size)
{
struct greybus_descriptor_header *desc_header = &desc->header;
size_t expected_size;
size_t desc_size;
if (size < sizeof(*desc_header)) {
gbsim_error("manifest too small\n");
return -EINVAL; /* Must at least have header */
}
desc_size = (int)le16toh(desc_header->size);
if ((size_t)desc_size > size) {
gbsim_error("descriptor too big\n");
return -EINVAL;
}
/* Descriptor needs to at least have a header */
expected_size = sizeof(*desc_header);
switch (desc_header->type) {
case GREYBUS_TYPE_STRING:
expected_size += sizeof(struct greybus_descriptor_string);
expected_size += desc->string.length;
/* String descriptors are padded to 4 byte boundaries */
expected_size = ALIGN(expected_size);
break;
case GREYBUS_TYPE_INTERFACE:
expected_size += sizeof(struct greybus_descriptor_interface);
break;
case GREYBUS_TYPE_BUNDLE:
expected_size += sizeof(struct greybus_descriptor_bundle);
break;
case GREYBUS_TYPE_CPORT:
expected_size += sizeof(struct greybus_descriptor_cport);
break;
case GREYBUS_TYPE_INVALID:
default:
gbsim_error("invalid descriptor type (%hhu)\n", desc_header->type);
return -EINVAL;
}
if (desc_size < expected_size) {
gbsim_error("%d descriptor too small (%zu < %zu)\n",
desc_header->type, desc_size, expected_size);
return -EINVAL;
}
/* Warn if there is a size mismatch */
if (desc_size != expected_size) {
gbsim_error("%d descriptor size mismatch, expected - %zu, actual - %zu)\n",
desc_header->type, expected_size, desc_size);
}
return desc_size;
}
/*
* Parse a buffer containing a Interface manifest.
*
* If we find anything wrong with the content/format of the buffer
* we reject it.
*
* The first requirement is that the manifest's version is
* one we can parse.
*
* We make an initial pass through the buffer and identify all of
* the descriptors it contains, keeping track for each its type
* and the location size of its data in the buffer.
*
* Next we scan the descriptors, looking for a interface descriptor;
* there must be exactly one of those. When found, we record the
* information it contains, and then remove that descriptor (and any
* string descriptors it refers to) from further consideration.
*
* After that we look for the interface's bundles--there must be at
* least one of those.
*
* Returns true if parsing was successful, false otherwise.
*/
bool manifest_parse(struct gbsim_svc *svc, int intf_id, void *data, size_t size)
{
struct gbsim_interface *intf;
struct greybus_manifest *manifest;
struct greybus_manifest_header *header;
struct greybus_descriptor *desc;
__u16 manifest_size;
/* we have to have at _least_ the manifest header */
if (size <= sizeof(manifest->header)) {
gbsim_error("short manifest (%zu)\n", size);
return false;
}
/* Make sure the size is right */
manifest = data;
header = &manifest->header;
manifest_size = le16toh(header->size);
if (manifest_size != size) {
gbsim_error("manifest size mismatch %zu != %hu\n",
size, manifest_size);
return false;
}
/* Validate major/minor number */
if (header->version_major > GREYBUS_VERSION_MAJOR) {
gbsim_error("manifest version too new (%hhu.%hhu > %d.%d)\n",
header->version_major, header->version_minor,
GREYBUS_VERSION_MAJOR, GREYBUS_VERSION_MINOR);
return false;
}
/* OK, find all the descriptors */
desc = (struct greybus_descriptor *)(header + 1);
size -= sizeof(*header);
intf = interface_get_by_id(svc, intf_id);
if (!intf)
return false;
intf->manifest = manifest;
intf->manifest_size = manifest_size;
while (size) {
int desc_size;
desc_size = identify_descriptor(intf, desc, size);
if (desc_size < 0)
return false;
desc = (struct greybus_descriptor *)((char *)desc + desc_size);
size -= desc_size;
}
return true;
}
/* Response from AP to Module, in response to a request Module has sent earlier */
static int firmware_handler_response(uint16_t cport_id, uint16_t hd_cport_id,
void *rbuf, size_t rsize)
{
struct op_msg *op_rsp = rbuf;
struct gb_operation_msg_hdr *oph = &op_rsp->header;
struct gb_firmware_size_response *size_response;
struct gb_firmware_get_firmware_response *get_fw_response;
int ret = 0;
int type = oph->type & ~OP_RESPONSE;
/* Did the request fail? */
if (oph->result) {
gbsim_error("%s: Operation type: %s FAILED (%d)\n", __func__,
firmware_get_operation(type), oph->result);
return oph->result;
}
switch (type) {
case GB_FIRMWARE_TYPE_FIRMWARE_SIZE:
size_response = &op_rsp->fw_size_resp;
firmware_size = le32toh(size_response->size);
gbsim_debug("%s: Firmware size returned is %d bytes\n",
__func__, firmware_size);
ret = fetch_firmware(hd_cport_id);
break;
case GB_FIRMWARE_TYPE_GET_FIRMWARE:
get_fw_response = &op_rsp->fw_get_firmware_resp;
ret = dump_firmware(get_fw_response->data);
if (ret)
break;
if (firmware_read_size < firmware_size) {
ret = fetch_firmware(hd_cport_id);
break;
}
ret = firmware_request_send(GB_FIRMWARE_TYPE_READY_TO_BOOT,
hd_cport_id);
if (ret)
gbsim_error("%s: Failed to send ready to boot message(%d)\n",
__func__, ret);
break;
case GB_FIRMWARE_TYPE_READY_TO_BOOT:
gbsim_debug("%s: AP granted permission to boot.\n", __func__);
break;
default:
gbsim_error("%s: Response not supported (%d)\n", __func__,
type);
return -EINVAL;
}
return ret;
}
int i2s_data_handler(uint16_t cport_id, uint16_t hd_cport_id, void *rbuf,
size_t rsize, void *tbuf, size_t tsize)
{
struct gb_operation_msg_hdr *oph;
struct op_msg *op_req = rbuf;
struct op_msg *op_rsp;
size_t payload_size;
uint16_t message_size;
uint8_t result = PROTOCOL_STATUS_SUCCESS;
op_rsp = (struct op_msg *)tbuf;
oph = (struct gb_operation_msg_hdr *)&op_req->header;
switch (oph->type) {
case GB_REQUEST_TYPE_PROTOCOL_VERSION:
payload_size = sizeof(struct gb_protocol_version_response);
op_rsp->pv_rsp.major = GREYBUS_VERSION_MAJOR;
op_rsp->pv_rsp.minor = GREYBUS_VERSION_MINOR;
break;
case GB_I2S_DATA_TYPE_SEND_DATA:
payload_size = 0;
break;
default:
gbsim_error("i2s data operation type %02x not supported\n", oph->type);
return -EINVAL;
}
message_size = sizeof(struct gb_operation_msg_hdr) + payload_size;
return send_response(op_rsp, hd_cport_id, message_size, oph, result);
}
/* Request from AP to Module */
static int firmware_handler_request(uint16_t cport_id, uint16_t hd_cport_id,
void *rbuf, size_t rsize, void *tbuf,
size_t tsize)
{
struct op_msg *op_req = rbuf;
struct op_msg *op_rsp = tbuf;
struct gb_operation_msg_hdr *oph = &op_req->header;
struct gb_protocol_version_response *version_request;
uint16_t message_size = sizeof(*oph);
size_t payload_size;
int ret;
switch (oph->type) {
case GB_REQUEST_TYPE_PROTOCOL_VERSION:
payload_size = sizeof(*version_request);
version_request = &op_req->fw_version_request;
gbsim_debug("AP Firmware version (%d %d)\n",
version_request->major, version_request->minor);
version_request = &op_rsp->fw_version_request;
version_request->major = GB_FIRMWARE_VERSION_MAJOR;
version_request->minor = GB_FIRMWARE_VERSION_MINOR;
break;
default:
gbsim_error("%s: Request not supported (%d)\n", __func__,
oph->type);
return -EINVAL;
}
message_size += payload_size;
ret = send_response(hd_cport_id, op_rsp, message_size,
oph->operation_id, oph->type,
PROTOCOL_STATUS_SUCCESS);
if (ret)
return ret;
ret = firmware_request_send(GB_FIRMWARE_TYPE_FIRMWARE_SIZE, hd_cport_id);
if (ret)
gbsim_error("%s: Failed to get size (%d)\n", __func__, ret);
return ret;
}
static int fetch_firmware(uint16_t hd_cport_id)
{
int ret;
ret = firmware_request_send(GB_FIRMWARE_TYPE_GET_FIRMWARE,
hd_cport_id);
if (ret)
gbsim_error("%s: Failed to get firmware (%d)\n", __func__, ret);
return ret;
}
void i2c_init(void)
{
char filename[20];
if (bbb_backend) {
snprintf(filename, 19, "/dev/i2c-%d", i2c_adapter);
ifd = open(filename, O_RDWR);
if (ifd < 0)
gbsim_error("failed opening i2c-dev node read/write\n");
}
}
/* Request from Module to AP */
int firmware_request_send(uint8_t type, uint16_t hd_cport_id)
{
struct op_msg msg = { };
struct gb_operation_msg_hdr *oph = &msg.header;
struct gb_firmware_size_request *size_request;
struct gb_firmware_get_firmware_request *get_fw_request;
struct gb_firmware_ready_to_boot_request *rbt_request;
uint16_t message_size = sizeof(*oph);
size_t payload_size;
switch (type) {
case GB_FIRMWARE_TYPE_FIRMWARE_SIZE:
payload_size = sizeof(*size_request);
size_request = &msg.fw_size_req;
size_request->stage = GB_FIRMWARE_BOOT_STAGE_ONE;
break;
case GB_FIRMWARE_TYPE_GET_FIRMWARE:
payload_size = sizeof(*get_fw_request);
get_fw_request = &msg.fw_get_firmware_req;
/* Calculate fetch size for remaining data */
firmware_fetch_size = firmware_size - firmware_read_size;
if (firmware_fetch_size > GB_FIRMWARE_FETCH_MAX)
firmware_fetch_size = GB_FIRMWARE_FETCH_MAX;
get_fw_request->offset = htole32(firmware_read_size);
get_fw_request->size = htole32(firmware_fetch_size);
break;
case GB_FIRMWARE_TYPE_READY_TO_BOOT:
payload_size = sizeof(*rbt_request);
rbt_request = &msg.fw_rbt_req;
rbt_request->status = GB_FIRMWARE_BOOT_STATUS_SECURE;
break;
default:
gbsim_error("firmware operation type %02x not supported\n",
type);
return -EINVAL;
}
message_size += payload_size;
return send_request(hd_cport_id, &msg, message_size, 1, type);
}
int control_handler(struct gbsim_cport *cport, void *rbuf,
size_t rsize, void *tbuf, size_t tsize)
{
struct op_msg *op_req = rbuf;
struct op_msg *op_rsp = tbuf;
struct gb_operation_msg_hdr *oph = &op_req->header;
size_t payload_size;
uint16_t message_size = sizeof(*oph);
uint16_t hd_cport_id = cport->hd_cport_id;
switch (oph->type) {
case GB_REQUEST_TYPE_PROTOCOL_VERSION:
payload_size = sizeof(op_rsp->pv_rsp);
op_rsp->pv_rsp.major = GB_CONTROL_VERSION_MAJOR;
op_rsp->pv_rsp.minor = GB_CONTROL_VERSION_MINOR;
break;
case GB_CONTROL_TYPE_GET_MANIFEST_SIZE:
payload_size = sizeof(op_rsp->control_msize_rsp);
op_rsp->control_msize_rsp.size = htole16(info.manifest_size);
break;
case GB_CONTROL_TYPE_GET_MANIFEST:
payload_size = info.manifest_size;
memcpy(&op_rsp->control_manifest_rsp.data, info.manifest,
payload_size);
break;
case GB_CONTROL_TYPE_CONNECTED:
payload_size = 0;
break;
case GB_CONTROL_TYPE_DISCONNECTED:
payload_size = 0;
break;
default:
gbsim_error("control operation type %02x not supported\n", oph->type);
return -EINVAL;
}
message_size += payload_size;
return send_response(hd_cport_id, op_rsp, message_size,
oph->operation_id, oph->type,
PROTOCOL_STATUS_SUCCESS);
}
void i2c_handler(__u8 *rbuf, size_t size)
{
struct op_header *oph;
char *tbuf;
struct op_msg *op_req, *op_rsp;
struct cport_msg *cport_req, *cport_rsp;
int i, op_count;
__u8 *write_data;
bool read_op;
int read_count = 0;
bool write_fail = false;
size_t sz;
tbuf = malloc(4 * 1024);
if (!tbuf) {
gbsim_error("failed to allocate i2c handler tx buf\n");
return;
}
cport_req = (struct cport_msg *)rbuf;
op_req = (struct op_msg *)cport_req->data;
cport_rsp = (struct cport_msg *)tbuf;
cport_rsp->cport = 0; /* FIXME hardcoded until we have connections */
op_rsp = (struct op_msg *)cport_rsp->data;
oph = (struct op_header *)&op_req->header;
switch (oph->type) {
case OP_I2C_PROTOCOL_VERSION:
sz = sizeof(struct op_header) +
sizeof(struct protocol_version_rsp);
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_VERSION;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
op_rsp->pv_rsp.version_major = GREYBUS_VERSION_MAJOR;
op_rsp->pv_rsp.version_minor = GREYBUS_VERSION_MINOR;
gbsim_debug("Module %d -> AP CPort %d I2C protocol version response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_FUNCTIONALITY:
sz = sizeof(struct op_header) +
sizeof(struct i2c_functionality_rsp);
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_FUNCTIONALITY;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
op_rsp->i2c_fcn_rsp.functionality = htole32(I2C_FUNC_I2C);
gbsim_debug("Module %d -> AP CPort %d I2C protocol functionality response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_TIMEOUT:
sz = sizeof(struct op_header) + 0;
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_TIMEOUT;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
gbsim_debug("Module %d -> AP CPort %d I2C protocol timeout response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_RETRIES:
sz = sizeof(struct op_header) + 0;
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_RETRIES;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
gbsim_debug("Module %d -> AP CPort %d I2C protocol retries response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_TRANSFER:
op_count = le16toh(op_req->i2c_xfer_req.op_count);
write_data = (__u8 *)&op_req->i2c_xfer_req.desc[op_count];
gbsim_debug("Number of transfer ops %d\n", op_count);
for (i = 0; i < op_count; i++) {
struct i2c_transfer_desc *desc;
__u16 addr;
__u16 flags;
__u16 size;
desc = &op_req->i2c_xfer_req.desc[i];
addr = le16toh(desc->addr);
flags = le16toh(desc->flags);
size = le16toh(desc->size);
read_op = (flags & I2C_M_RD) ? true : false;
gbsim_debug("op %d: %s address %04x size %04x\n",
i, (read_op ? "read" : "write"),
addr, size);
/* FIXME: need some error handling */
if (bbb_backend)
if (ioctl(ifd, I2C_SLAVE, addr) < 0)
gbsim_error("failed setting i2c slave address\n");
if (read_op) {
if (bbb_backend) {
int count;
ioctl(ifd, BLKFLSBUF);
count = read(ifd, &op_rsp->i2c_xfer_rsp.data[read_count], size);
if (count != size)
gbsim_error("op %d: failed to read %04x bytes\n", i, size);
} else {
for (i = read_count; i < (read_count + size); i++)
op_rsp->i2c_xfer_rsp.data[i] = data_byte++;
}
read_count += size;
} else {
if (bbb_backend) {
int count;
count = write(ifd, write_data, size);
if (count != size) {
gbsim_debug("op %d: failed to write %04x bytes\n", i, size);
write_fail = true;
}
}
write_data += size;
}
}
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_TRANSFER;
if (write_fail)
op_rsp->header.result = PROTOCOL_STATUS_RETRY;
else
/* FIXME: handle read failure */
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
if (read_op)
sz = sizeof(struct op_header) + 1 + read_count;
else
sz = sizeof(struct op_header) + 1;
op_rsp->header.size = htole16((__u16)sz);
gbsim_debug("Module %d -> AP CPort %d I2C transfer response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
default:
gbsim_error("i2c operation type %02x not supported\n", oph->type);
}
free(tbuf);
}
int i2c_handler(uint16_t cport_id, uint16_t hd_cport_id, void *rbuf,
size_t rsize, void *tbuf, size_t tsize)
{
struct gb_operation_msg_hdr *oph;
struct op_msg *op_req = rbuf;
struct op_msg *op_rsp;
int i, op_count;
__u8 *write_data;
bool read_op = false;
int read_count = 0;
bool write_fail = false;
size_t payload_size;
uint16_t message_size;
uint8_t result = PROTOCOL_STATUS_SUCCESS;
op_rsp = (struct op_msg *)tbuf;
oph = (struct gb_operation_msg_hdr *)&op_req->header;
switch (oph->type) {
case GB_REQUEST_TYPE_PROTOCOL_VERSION:
payload_size = sizeof(struct gb_protocol_version_response);
op_rsp->pv_rsp.major = GREYBUS_VERSION_MAJOR;
op_rsp->pv_rsp.minor = GREYBUS_VERSION_MINOR;
break;
case GB_I2C_TYPE_FUNCTIONALITY:
payload_size = sizeof(struct gb_i2c_functionality_response);
op_rsp->i2c_fcn_rsp.functionality = htole32(I2C_FUNC_I2C);
break;
case GB_I2C_TYPE_TIMEOUT:
payload_size = 0;
break;
case GB_I2C_TYPE_RETRIES:
payload_size = 0;
break;
case GB_I2C_TYPE_TRANSFER:
op_count = le16toh(op_req->i2c_xfer_req.op_count);
write_data = (__u8 *)&op_req->i2c_xfer_req.ops[op_count];
gbsim_debug("Number of transfer ops %d\n", op_count);
for (i = 0; i < op_count; i++) {
struct gb_i2c_transfer_op *op;
__u16 addr;
__u16 flags;
__u16 size;
op = &op_req->i2c_xfer_req.ops[i];
addr = le16toh(op->addr);
flags = le16toh(op->flags);
size = le16toh(op->size);
read_op = (flags & I2C_M_RD) ? true : false;
gbsim_debug("op %d: %s address %04x size %04x\n",
i, (read_op ? "read" : "write"),
addr, size);
/* FIXME: need some error handling */
if (bbb_backend)
if (ioctl(ifd, I2C_SLAVE, addr) < 0)
gbsim_error("failed setting i2c slave address\n");
if (read_op) {
if (bbb_backend) {
int count;
ioctl(ifd, BLKFLSBUF);
count = read(ifd, &op_rsp->i2c_xfer_rsp.data[read_count], size);
if (count != size)
gbsim_error("op %d: failed to read %04x bytes\n", i, size);
} else {
for (i = read_count; i < (read_count + size); i++)
op_rsp->i2c_xfer_rsp.data[i] = data_byte++;
}
read_count += size;
} else {
if (bbb_backend) {
int count;
count = write(ifd, write_data, size);
if (count != size) {
gbsim_debug("op %d: failed to write %04x bytes\n", i, size);
write_fail = true;
}
}
write_data += size;
}
}
/* FIXME: handle read failure */
if (write_fail)
result = PROTOCOL_STATUS_RETRY;
payload_size = read_op ? read_count : 0;
break;
default:
return -EINVAL;
}
message_size = sizeof(struct gb_operation_msg_hdr) + payload_size;
return send_response(op_rsp, hd_cport_id, message_size, oph, result);
}
int control_handler(struct gbsim_connection *connection, void *rbuf,
size_t rsize, void *tbuf, size_t tsize)
{
struct op_msg *op_req = rbuf;
struct op_msg *op_rsp = tbuf;
struct gb_operation_msg_hdr *oph = &op_req->header;
struct gbsim_interface *intf = connection->intf;
size_t payload_size;
uint16_t message_size = sizeof(*oph);
uint16_t hd_cport_id = connection->hd_cport_id;
switch (oph->type) {
case GB_REQUEST_TYPE_CPORT_SHUTDOWN:
payload_size = 0;
break;
case GB_CONTROL_TYPE_VERSION:
payload_size = sizeof(op_rsp->control_version_rsp);
op_rsp->control_version_rsp.major = GBSIM_CONTROL_VERSION_MAJOR;
op_rsp->control_version_rsp.minor = GBSIM_CONTROL_VERSION_MINOR;
break;
case GB_CONTROL_TYPE_GET_MANIFEST_SIZE:
payload_size = sizeof(op_rsp->control_msize_rsp);
op_rsp->control_msize_rsp.size = htole16(intf->manifest_size);
break;
case GB_CONTROL_TYPE_GET_MANIFEST:
payload_size = intf->manifest_size;
memcpy(&op_rsp->control_manifest_rsp.data, intf->manifest,
payload_size);
break;
case GB_CONTROL_TYPE_CONNECTED:
payload_size = 0;
break;
case GB_CONTROL_TYPE_BUNDLE_ACTIVATE:
payload_size = sizeof(op_rsp->control_bundle_pm_rsp);
op_rsp->control_bundle_pm_rsp.status = GB_CONTROL_BUNDLE_PM_OK;
break;
case GB_CONTROL_TYPE_BUNDLE_SUSPEND:
payload_size = sizeof(op_rsp->control_bundle_pm_rsp);
op_rsp->control_bundle_pm_rsp.status = GB_CONTROL_BUNDLE_PM_OK;
break;
case GB_CONTROL_TYPE_BUNDLE_RESUME:
payload_size = sizeof(op_rsp->control_bundle_pm_rsp);
op_rsp->control_bundle_pm_rsp.status = GB_CONTROL_BUNDLE_PM_OK;
break;
case GB_CONTROL_TYPE_DISCONNECTED:
payload_size = 0;
break;
case GB_CONTROL_TYPE_INTF_SUSPEND_PREPARE:
payload_size = sizeof(op_rsp->control_intf_pm_rsp);
op_rsp->control_intf_pm_rsp.status = GB_CONTROL_INTF_PM_OK;
break;
case GB_CONTROL_TYPE_DISCONNECTING:
payload_size = 0;
break;
default:
gbsim_error("control operation type %02x not supported\n", oph->type);
return -EINVAL;
}
message_size += payload_size;
return send_response(hd_cport_id, op_rsp, message_size,
oph->operation_id, oph->type,
PROTOCOL_STATUS_SUCCESS);
}
int pwm_handler(struct gbsim_cport *cport, void *rbuf,
size_t rsize, void *tbuf, size_t tsize)
{
struct gb_operation_msg_hdr *oph;
struct op_msg *op_req = rbuf;
struct op_msg *op_rsp;
__u32 duty;
__u32 period;
size_t payload_size;
uint16_t message_size;
uint16_t hd_cport_id = cport->hd_cport_id;
uint8_t result = PROTOCOL_STATUS_SUCCESS;
op_rsp = (struct op_msg *)tbuf;
oph = (struct gb_operation_msg_hdr *)&op_req->header;
switch (oph->type) {
case GB_REQUEST_TYPE_PROTOCOL_VERSION:
payload_size = sizeof(struct gb_protocol_version_response);
op_rsp->pv_rsp.major = GREYBUS_VERSION_MAJOR;
op_rsp->pv_rsp.minor = GREYBUS_VERSION_MINOR;
break;
case GB_PWM_TYPE_PWM_COUNT:
payload_size = sizeof(struct gb_pwm_count_response);
op_rsp->pwm_cnt_rsp.count = 1; /* Something arbitrary, but useful */
break;
case GB_PWM_TYPE_ACTIVATE:
payload_size = 0;
gbsim_debug("PWM %d activate request\n ",
op_req->pwm_act_req.which);
break;
case GB_PWM_TYPE_DEACTIVATE:
payload_size = 0;
gbsim_debug("PWM %d deactivate request\n ",
op_req->pwm_deact_req.which);
break;
case GB_PWM_TYPE_CONFIG:
payload_size = 0;
duty = le32toh(op_req->pwm_cfg_req.duty);
period = le32toh(op_req->pwm_cfg_req.period);
if (bbb_backend) {
libsoc_pwm_set_duty_cycle(pwms[op_req->pwm_cfg_req.which], duty);
libsoc_pwm_set_period(pwms[op_req->pwm_cfg_req.which], period);
}
gbsim_debug("PWM %d config (%dns/%dns) request\n ",
op_req->pwm_cfg_req.which, duty, period);
break;
case GB_PWM_TYPE_POLARITY:
payload_size = 0;
if (pwm_on[op_req->pwm_pol_req.which]) {
result = PROTOCOL_STATUS_BUSY;
} else if (bbb_backend) {
libsoc_pwm_set_polarity(pwms[op_req->pwm_pol_req.which],
op_req->pwm_pol_req.polarity);
}
gbsim_debug("PWM %d polarity (%s) request\n ",
op_req->pwm_cfg_req.which,
op_req->pwm_pol_req.polarity ? "inverse" : "normal");
break;
case GB_PWM_TYPE_ENABLE:
payload_size = 0;
pwm_on[op_req->pwm_enb_req.which] = 1;
if (bbb_backend)
libsoc_pwm_set_enabled(pwms[op_req->pwm_enb_req.which], ENABLED);
gbsim_debug("PWM %d enable request\n ",
op_req->pwm_enb_req.which);
break;
case GB_PWM_TYPE_DISABLE:
payload_size = 0;
pwm_on[op_req->pwm_dis_req.which] = 0;
if (bbb_backend)
libsoc_pwm_set_enabled(pwms[op_req->pwm_dis_req.which], DISABLED);
gbsim_debug("PWM %d disable request\n ",
op_req->pwm_dis_req.which);
break;
default:
gbsim_error("pwm operation type %02x not supported\n", oph->type);
return -EINVAL;
}
message_size = sizeof(struct gb_operation_msg_hdr) + payload_size;
return send_response(hd_cport_id, op_rsp, message_size,
oph->operation_id, oph->type, result);
}
int i2s_mgmt_handler(uint16_t cport_id, uint16_t hd_cport_id, void *rbuf,
size_t rsize, void *tbuf, size_t tsize)
{
struct gb_operation_msg_hdr *oph;
struct op_msg *op_req = rbuf;
struct op_msg *op_rsp;
struct gb_i2s_mgmt_configuration *conf;
size_t payload_size;
uint16_t message_size;
uint8_t result = PROTOCOL_STATUS_SUCCESS;
op_rsp = (struct op_msg *)tbuf;
oph = (struct gb_operation_msg_hdr *)&op_req->header;
switch (oph->type) {
case GB_REQUEST_TYPE_PROTOCOL_VERSION:
payload_size = sizeof(struct gb_protocol_version_response);
op_rsp->pv_rsp.major = GREYBUS_VERSION_MAJOR;
op_rsp->pv_rsp.minor = GREYBUS_VERSION_MINOR;
break;
case GB_I2S_MGMT_TYPE_GET_SUPPORTED_CONFIGURATIONS:
payload_size = sizeof(struct gb_i2s_mgmt_get_supported_configurations_response) +
sizeof(struct gb_i2s_mgmt_configuration) * CONFIG_COUNT_MAX;
op_rsp->i2s_mgmt_get_sup_conf_rsp.config_count = 1;
conf = &op_rsp->i2s_mgmt_get_sup_conf_rsp.config[0];
conf->sample_frequency = htole32(48000);
conf->num_channels = 2;
conf->bytes_per_channel = 2;
conf->byte_order = GB_I2S_MGMT_BYTE_ORDER_LE;
conf->spatial_locations = htole32(
GB_I2S_MGMT_SPATIAL_LOCATION_FL |
GB_I2S_MGMT_SPATIAL_LOCATION_FR);
conf->ll_protocol = htole32(GB_I2S_MGMT_PROTOCOL_I2S);
conf->ll_mclk_role = GB_I2S_MGMT_ROLE_MASTER;
conf->ll_bclk_role = GB_I2S_MGMT_ROLE_MASTER;
conf->ll_wclk_role = GB_I2S_MGMT_ROLE_MASTER;
conf->ll_wclk_polarity = GB_I2S_MGMT_POLARITY_NORMAL;
conf->ll_wclk_change_edge = GB_I2S_MGMT_EDGE_FALLING;
conf->ll_wclk_tx_edge = GB_I2S_MGMT_EDGE_RISING;
conf->ll_wclk_rx_edge = GB_I2S_MGMT_EDGE_FALLING;
conf->ll_data_offset = 1;
break;
case GB_I2S_MGMT_TYPE_SET_CONFIGURATION:
payload_size = 0;
break;
case GB_I2S_MGMT_TYPE_SET_SAMPLES_PER_MESSAGE:
payload_size = 0;
break;
case GB_I2S_MGMT_TYPE_SET_START_DELAY:
payload_size = 0;
break;
case GB_I2S_MGMT_TYPE_ACTIVATE_CPORT:
payload_size = 0;
break;
case GB_I2S_MGMT_TYPE_DEACTIVATE_CPORT:
payload_size = 0;
break;
default:
gbsim_error("i2s mgmt operation type %02x not supported\n", oph->type);
return -EINVAL;
}
message_size = sizeof(struct gb_operation_msg_hdr) + payload_size;
return send_response(op_rsp, hd_cport_id, message_size, oph, result);
}