void vchiq_terminate_service_internal(VCHIQ_SERVICE_T *service)
{
VCHIQ_CHANNEL_T *remote = service->state->remote;
int remove, fourcc;
/* Release any unreleased messages */
remove = remote->ctrl.remove;
VCHIQ_TRACE("%d: tsi - (%d<->%d) i %x, p %x, r %x", service->state->id, service->localport, service->remoteport, remote->ctrl.insert, remote->ctrl.process, remove);
fourcc = VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA, service->remoteport, service->localport);
while (remove != remote->ctrl.insert)
{
VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)(remote->ctrl.data + (remove & VCHIQ_CHANNEL_MASK));
remove += calc_stride(header->size);
if (header->fourcc == fourcc)
header->fourcc = VCHIQ_FOURCC_INVALID;
}
/* Mark the service for termination by the slot handler... */
service->terminate = 1;
/* ... and ensure the slot handler runs. */
remote_event_signal_local(&service->state->local->trigger);
}
VCHIQ_STATUS_T vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T handle)
{
/* Unregister the service */
VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
VCHIQ_STATE_T *state = service->state;
VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
switch (service->srvstate)
{
case VCHIQ_SRVSTATE_OPENING:
case VCHIQ_SRVSTATE_OPEN:
{
int oldstate = service->srvstate;
/* Start the CLOSE procedure */
vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
status = queue_message(state,
VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, service->localport, service->remoteport),
NULL, 0, 0);
if (status != VCHIQ_SUCCESS)
vchiq_set_service_state(service, oldstate);
}
break;
case VCHIQ_SRVSTATE_HIDDEN:
case VCHIQ_SRVSTATE_LISTENING:
case VCHIQ_SRVSTATE_CLOSING:
case VCHIQ_SRVSTATE_CLOSEWAIT:
break;
default:
status = VCHIQ_ERROR;
break;
}
while ((service->srvstate == VCHIQ_SRVSTATE_CLOSING) ||
(service->srvstate == VCHIQ_SRVSTATE_CLOSESENT))
{
if (!local_event_wait(&service->remove_event))
{
status = VCHIQ_RETRY;
break;
}
}
if (status == VCHIQ_SUCCESS)
{
if (service->srvstate == VCHIQ_SRVSTATE_OPEN)
status = VCHIQ_ERROR;
else
vchiq_set_service_state(service, VCHIQ_SRVSTATE_FREE);
}
return status;
}
static void notify_tx_bulks(VCHIQ_STATE_T *state)
{
VCHIQ_CHANNEL_T *local = state->local;
int i;
VCHIQ_TRACE("%d: ntb", state->id);
for (i = 0; i < VCHIQ_MAX_SERVICES; i++)
{
VCHIQ_SERVICE_T *service = &local->services[i];
if (service->srvstate > VCHIQ_SRVSTATE_LISTENING)
VCHIQ_TRACE("%d: ntb - srv %d state %d", state->id, service->localport, service->srvstate);
if (service->terminate)
{
if ((service->srvstate == VCHIQ_SRVSTATE_OPENING) ||
(service->srvstate == VCHIQ_SRVSTATE_OPEN))
{
if (queue_message(state,
VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, service->localport, service->remoteport),
NULL, 0, 0) == VCHIQ_RETRY)
continue;
}
service->terminate = 0;
vchiq_set_service_state(service, VCHIQ_SRVSTATE_FREE);
}
else if (service->srvstate == VCHIQ_SRVSTATE_OPEN)
{
while (service->remove != service->process) {
VCHIQ_TX_BULK_T *bulk = &service->bulks[service->remove & (VCHIQ_NUM_SERVICE_BULKS - 1)];
VCHIQ_TRACE("%d: ntb %x DONE", state->id, service->remove);
if (service->base.callback(bulk->data ? VCHIQ_BULK_TRANSMIT_DONE : VCHIQ_BULK_TRANSMIT_ABORTED,
NULL, &service->base, bulk->userdata) == VCHIQ_RETRY)
break; /* Bail out if not ready */
service->remove++;
local_event_signal(&service->remove_event);
}
}
}
}
VCHIQ_STATUS_T vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T handle, const VCHIQ_ELEMENT_T *elements, int count)
{
VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
unsigned int size = 0;
unsigned int i;
if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
return VCHIQ_ERROR;
for (i = 0; i < (unsigned int)count; i++)
size += elements[i].size;
if (calc_stride(size) > VCHIQ_CHANNEL_SIZE)
return VCHIQ_ERROR;
return queue_message(service->state,
VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA, service->localport, service->remoteport),
elements, count, size);
}
VCHIQ_STATUS_T vchiq_open_service_internal(VCHIQ_SERVICE_T *service)
{
VCHIQ_ELEMENT_T body = { &service->base.fourcc, sizeof(service->base.fourcc) };
VCHIQ_STATUS_T status = queue_message(service->state,
VCHIQ_MAKE_MSG(VCHIQ_MSG_OPEN, service->localport, 0),
&body, 1, sizeof(service->base.fourcc));
if (status == VCHIQ_SUCCESS)
{
if (!local_event_wait(&service->remove_event))
{
status = VCHIQ_RETRY;
}
else if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
{
VCHIQ_TRACE("%d: osi - srvstate = %d", service->state->id, service->srvstate);
vcos_assert(service->srvstate == VCHIQ_SRVSTATE_CLOSEWAIT);
status = VCHIQ_ERROR;
}
}
return status;
}
VCHIQ_STATUS_T vchiq_connect_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance)
{
VCHIQ_CHANNEL_T *local = state->local;
int i;
/* Find all services registered to this client and enable them. */
for (i = 0; i < VCHIQ_MAX_SERVICES; i++)
if (local->services[i].instance == instance)
{
if (local->services[i].srvstate == VCHIQ_SRVSTATE_HIDDEN)
vchiq_set_service_state(&local->services[i], VCHIQ_SRVSTATE_LISTENING);
}
if (!state->connected)
{
if (queue_message(state, VCHIQ_MAKE_MSG(VCHIQ_MSG_CONNECT, 0, 0), NULL, 0, 0) == VCHIQ_RETRY)
return VCHIQ_RETRY;
vcos_event_wait(&state->connect);
state->connected = 1;
}
return VCHIQ_SUCCESS;
}
static void parse_rx_slots(VCHIQ_STATE_T *state)
{
VCHIQ_CHANNEL_T *remote = state->remote;
VCHIQ_CHANNEL_T *local = state->local;
while (remote->ctrl.process != remote->ctrl.insert) {
VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)(remote->ctrl.data + (remote->ctrl.process & VCHIQ_CHANNEL_MASK));
VCHIQ_SERVICE_T *service = NULL;
unsigned int stride = calc_stride(header->size);
int type = VCHIQ_MSG_TYPE(header->fourcc);
VCHIQ_TRACE("%d: prs %d (%d,%d)", state->id, type, VCHIQ_MSG_DSTPORT(header->fourcc), VCHIQ_MSG_SRCPORT(header->fourcc));
switch (type)
{
case VCHIQ_MSG_OPEN:
vcos_assert(VCHIQ_MSG_DSTPORT(header->fourcc) == 0);
if (vcos_verify(header->size == 4))
{
VCHIQ_HEADER_T *reply;
unsigned short remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
int target;
service = get_listening_service(local, *(int *)header->data);
local_mutex_acquire(&local->ctrl.mutex);
target = local->ctrl.insert + sizeof(VCHIQ_HEADER_T);
reply = reserve_space(local, target);
if (!reply)
{
local_mutex_release(&local->ctrl.mutex);
return; /* Bail out */
}
if (service && (service->srvstate == VCHIQ_SRVSTATE_LISTENING))
{
/* A matching, listening service exists - attempt the OPEN */
VCHIQ_STATUS_T status;
vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN); /* Proceed as if the connection will be accepted */
status = service->base.callback(VCHIQ_SERVICE_OPENED, NULL, &service->base, NULL);
if (status == VCHIQ_SUCCESS)
{
/* The open was accepted - acknowledge it */
reply->fourcc = VCHIQ_MAKE_MSG(VCHIQ_MSG_OPENACK, service->localport, remoteport);
service->remoteport = remoteport;
}
else
{
vchiq_set_service_state(service, VCHIQ_SRVSTATE_LISTENING);
if (status == VCHIQ_RETRY)
return; /* Bail out if not ready */
/* The open was rejected - send a close */
reply->fourcc = VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, 0, remoteport);
}
}
else
{
/* No matching, available service - send a CLOSE */
reply->fourcc = VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, 0, remoteport);
}
reply->size = 0;
local->ctrl.insert = target;
local_mutex_release(&local->ctrl.mutex);
remote_event_signal(&remote->trigger);
}
break;
case VCHIQ_MSG_OPENACK:
{
unsigned int localport = VCHIQ_MSG_DSTPORT(header->fourcc);
unsigned int remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
service = &local->services[localport];
if (vcos_verify((localport < VCHIQ_MAX_SERVICES) &&
(service->srvstate == VCHIQ_SRVSTATE_OPENING)))
{
service->remoteport = remoteport;
vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN);
local_event_signal(&service->remove_event);
}
}
break;
case VCHIQ_MSG_CLOSE:
{
unsigned int localport = VCHIQ_MSG_DSTPORT(header->fourcc);
unsigned int remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
service = &local->services[localport];
vcos_assert(header->size == 0); /* There should be no data */
if (vcos_verify(localport < VCHIQ_MAX_SERVICES))
{
switch (service->srvstate)
{
case VCHIQ_SRVSTATE_OPEN:
if (service->remoteport != remoteport)
break;
/* Return the close */
if (queue_message(state,
VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, service->localport, service->remoteport),
NULL, 0, 0) == VCHIQ_RETRY)
return; /* Bail out if not ready */
vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
/* Drop through... */
case VCHIQ_SRVSTATE_CLOSESENT:
if (service->remoteport != remoteport)
break;
vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSING);
/* Drop through... */
case VCHIQ_SRVSTATE_CLOSING:
if (service->remoteport == remoteport)
{
/* Start the close procedure */
if (vchiq_close_service_internal(service) == VCHIQ_RETRY)
return; /* Bail out if not ready */
}
break;
case VCHIQ_SRVSTATE_OPENING:
/* A client is mid-open - this is a rejection, so just fail the open */
vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSEWAIT);
local_event_signal(&service->remove_event);
break;
default:
break;
}
}
}
break;
case VCHIQ_MSG_DATA:
{
unsigned int localport = VCHIQ_MSG_DSTPORT(header->fourcc);
unsigned int remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
service = &local->services[localport];
if (vcos_verify((localport < VCHIQ_MAX_SERVICES) &&
(service->remoteport == remoteport)) &&
(service->srvstate == VCHIQ_SRVSTATE_OPEN))
{
if (service->base.callback(VCHIQ_MESSAGE_AVAILABLE, header, &service->base, NULL) == VCHIQ_RETRY)
return; /* Bail out if not ready */
header = NULL; /* Don't invalidate this message - defer till vchiq_release */
}
}
break;
case VCHIQ_MSG_CONNECT:
vcos_event_signal(&state->connect);
break;
case VCHIQ_MSG_INVALID:
default:
break;
}
remote->ctrl.process += stride;
if (header != NULL)
{
/* Invalidate it */
header->fourcc = VCHIQ_FOURCC_INVALID;
/* Notify the other end there is some space */
remote_event_signal(&remote->ctrl.remove_event);
}
}
}
VCHIQ_PAGED_SEGMENT_BEGIN
/*++
Routine Description:
VchiqIoDeviceControl handles VCHIQ IOCTL.
Arguments:
DeviceContextPtr - Pointer to device context
WdfRequest - A handle to a framework request object.
OutputBufferLength - The length, in bytes, of the request's output buffer,
if an output buffer is available.
InputBufferLength - The length, in bytes, of the request's input buffer,
if an input buffer is available.
IoControlCode - The driver-defined or system-defined I/O control code
(IOCTL) that is associated with the request.
Return Value:
None
--*/
_Use_decl_annotations_
VOID VchiqIoDeviceControl (
WDFQUEUE Queue,
WDFREQUEST WdfRequest,
size_t OutputBufferLength,
size_t InputBufferLength,
ULONG IoControlCode
)
{
NTSTATUS status;
VCHIQ_FILE_CONTEXT* vchiqFileContextPtr;
WDFDEVICE device = WdfIoQueueGetDevice(Queue);
DEVICE_CONTEXT* deviceContextPtr = VchiqGetDeviceContext(device);
PAGED_CODE();
if (deviceContextPtr->VCConnected == FALSE) {
VCHIQ_LOG_WARNING("VCHIQ interface not ready");
status = STATUS_DEVICE_NOT_READY;
goto CompleteRequest;
}
// First process IOCTL that does not require a file context
switch (IoControlCode)
{
case IOCTL_VCHIQ_CONNECT:
{
// Connect is a simple ioctl to ensure that we have establish
// connection with VC firmware. Connection is establish by
// initiating the slots which if we reach this point has
// already been initialize. We just send a message to notify
// the firmware too
WDFFILEOBJECT wdfFileObject = WdfRequestGetFileObject(WdfRequest);
if (wdfFileObject == NULL) {
WDF_REQUEST_PARAMETERS wdfRequestParameters;
WdfRequestGetParameters(WdfRequest, &wdfRequestParameters);
VCHIQ_LOG_ERROR(
"Fail to retrieve file object. \
(WdfRequest = 0x%p, Type = 0x%lx)",
WdfRequest,
(ULONG)wdfRequestParameters.Type);
status = STATUS_INTERNAL_ERROR;
goto CompleteRequest;
}
// Create a file context here as vchiq_arm would immediately begin
// to send IOCTL to wait for completion message
vchiqFileContextPtr = VchiqGetFileContext(wdfFileObject);
if (vchiqFileContextPtr != NULL) {
VCHIQ_LOG_ERROR(
"Caller has already connected to a service");
status = STATUS_UNSUCCESSFUL;
goto CompleteRequest;
}
status = VchiqAllocateFileObjContext(
deviceContextPtr,
wdfFileObject,
&vchiqFileContextPtr);
if (!NT_SUCCESS (status)) {
VCHIQ_LOG_ERROR(
"VchiqAllocateFileObjContext failed (%!STATUS!)",
status);
goto CompleteRequest;
}
status = VchiqQueueMessageAsync(
deviceContextPtr,
vchiqFileContextPtr,
VCHIQ_MAKE_MSG(VCHIQ_MSG_CONNECT, 0, 0),
NULL,
0);
if (!NT_SUCCESS (status)) {
VCHIQ_LOG_ERROR(
"VchiqQueueMessageAsync failed (%!STATUS!)",
status);
goto CompleteRequest;
}
goto CompleteRequest;
}
case IOCTL_VCHIQ_GET_CONFIG:
{
VCHIQ_CONFIG vchiqCurrentConfig =
{
VCHIQ_MAX_MSG_SIZE,
VCHIQ_MAX_MSG_SIZE,
VCHIQ_NUM_SERVICE_BULKS,
VCHIQ_MAX_SERVICES,
VCHIQ_VERSION,
VCHIQ_VERSION_MIN
};
VCHIQ_GET_CONFIG* clientConfigPtr;
status = WdfRequestRetrieveInputBuffer(
WdfRequest,
sizeof(*clientConfigPtr),
&clientConfigPtr,
NULL);
if (!NT_SUCCESS(status)) {
VCHIQ_LOG_ERROR(
"WdfRequestRetrieveInputBuffer failed (%!STATUS!)",
status);
goto CompleteRequest;
}
// Ensure that the buffer provided is not too big.
if (clientConfigPtr->ConfigSize > sizeof(VCHIQ_CONFIG)) {
VCHIQ_LOG_WARNING("Config buffer too big");
status = STATUS_INVALID_PARAMETER;
goto CompleteRequest;
}
VCHIQ_CONFIG* configurationPtr;
size_t bufferSize;
configurationPtr = WdfMemoryGetBuffer(
clientConfigPtr->WdfMemoryConfiguration,
&bufferSize);
if ((configurationPtr == NULL) ||
(bufferSize != sizeof(*configurationPtr))) {
VCHIQ_LOG_ERROR(
"Caller provided invalid VCHIQ_CONFIG buffer 0x%08x %d",
(ULONG)configurationPtr,
bufferSize);
status = STATUS_INVALID_PARAMETER;
goto CompleteRequest;
}
RtlCopyMemory(
configurationPtr,
&vchiqCurrentConfig,
sizeof(*clientConfigPtr->PConfig));
status = STATUS_SUCCESS;
}
goto CompleteRequest;
case IOCTL_VCHIQ_LIB_VERSION:
{
ULONG* libVersion;
status = WdfRequestRetrieveInputBuffer(
WdfRequest,
sizeof(*libVersion),
&libVersion,
NULL);
if (!NT_SUCCESS(status)) {
VCHIQ_LOG_ERROR(
"WdfRequestRetrieveInputBuffer failed (%!STATUS!)",
status);
goto CompleteRequest;
}
if (*libVersion < VCHIQ_VERSION_MIN) {
VCHIQ_LOG_ERROR(
"Library version %d unsupported",
*libVersion);
status = STATUS_NOT_SUPPORTED;
goto CompleteRequest;
}
status = STATUS_SUCCESS;
}
goto CompleteRequest;
case IOCTL_VCHIQ_CREATE_SERVICE:
default:
{
WDFFILEOBJECT wdfFileObject = WdfRequestGetFileObject(WdfRequest);
if (wdfFileObject == NULL) {
WDF_REQUEST_PARAMETERS wdfRequestParameters;
WdfRequestGetParameters(WdfRequest, &wdfRequestParameters);
VCHIQ_LOG_ERROR(
"Fail to retrieve file object. \
(WdfRequest = 0x%p, Type = 0x%lx)",
WdfRequest,
(ULONG)wdfRequestParameters.Type);
status = STATUS_INTERNAL_ERROR;
goto CompleteRequest;
}
vchiqFileContextPtr = VchiqGetFileContext(wdfFileObject);
if ((vchiqFileContextPtr == NULL) &&
(IoControlCode == IOCTL_VCHIQ_CREATE_SERVICE)) {
// Functional test does not call connect prior to a create
// service call, so we allocate a file context here instead
status = VchiqAllocateFileObjContext(
deviceContextPtr,
wdfFileObject,
&vchiqFileContextPtr);
if (!NT_SUCCESS (status)) {
VCHIQ_LOG_ERROR(
"VchiqAllocateFileObjContext failed (%!STATUS!)",
status);
goto CompleteRequest;
}
} else if (vchiqFileContextPtr == NULL) {
VCHIQ_LOG_ERROR(
"Caller has not connected to a service %d",
((IoControlCode >> 2) & 0x0FFF));
status = STATUS_UNSUCCESSFUL;
goto CompleteRequest;
}
}
}