BOOLEAN
SynchronizedReadWriteRoutine(
IN PVOID DeviceExtension,
IN PVOID Context
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PSCSI_REQUEST_BLOCK Srb = (PSCSI_REQUEST_BLOCK) Context;
PRHEL_SRB_EXTENSION srbExt = (PRHEL_SRB_EXTENSION)Srb->SrbExtension;
PVOID va;
ULONGLONG pa;
SET_VA_PA();
if (virtqueue_add_buf(adaptExt->vq,
&srbExt->vbr.sg[0],
srbExt->out, srbExt->in,
&srbExt->vbr, va, pa) >= 0){
InsertTailList(&adaptExt->list_head, &srbExt->vbr.list_entry);
virtqueue_kick(adaptExt->vq);
return TRUE;
}
virtqueue_kick(adaptExt->vq);
StorPortBusy(DeviceExtension, 2);
return FALSE;
}
BOOLEAN
SynchronizedTMFRoutine(
IN PVOID DeviceExtension,
IN PVOID Context
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PSCSI_REQUEST_BLOCK Srb = (PSCSI_REQUEST_BLOCK) Context;
PSRB_EXTENSION srbExt = SRB_EXTENSION(Srb);
PVOID va;
ULONGLONG pa;
ENTER_FN();
SET_VA_PA();
if (virtqueue_add_buf(adaptExt->vq[VIRTIO_SCSI_CONTROL_QUEUE],
&srbExt->sg[0],
srbExt->out, srbExt->in,
&srbExt->cmd, va, pa) >= 0){
virtqueue_kick(adaptExt->vq[VIRTIO_SCSI_CONTROL_QUEUE]);
return TRUE;
}
SRB_SET_SRB_STATUS(Srb, SRB_STATUS_BUSY);
StorPortBusy(DeviceExtension, adaptExt->queue_depth);
EXIT_ERR();
return FALSE;
}
NTSTATUS
VIOSerialAddInBuf(
IN struct virtqueue *vq,
IN PPORT_BUFFER buf)
{
NTSTATUS status = STATUS_SUCCESS;
struct VirtIOBufferDescriptor sg;
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_QUEUEING, "--> %s buf = %p\n", __FUNCTION__, buf);
if (buf == NULL)
{
ASSERT(0);
return STATUS_INSUFFICIENT_RESOURCES;
}
if (vq == NULL)
{
ASSERT(0);
return STATUS_INSUFFICIENT_RESOURCES;
}
sg.physAddr = buf->pa_buf;
sg.length = buf->size;
if(0 > virtqueue_add_buf(vq, &sg, 0, 1, buf, NULL, 0))
{
TraceEvents(TRACE_LEVEL_ERROR, DBG_QUEUEING, "<-- %s cannot add_buf\n", __FUNCTION__);
status = STATUS_INSUFFICIENT_RESOURCES;
}
virtqueue_kick(vq);
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_QUEUEING, "<-- %s\n", __FUNCTION__);
return status;
}
VOID
RhelGetSerialNumber(
IN PVOID DeviceExtension
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
adaptExt->vbr.out_hdr.type = VIRTIO_BLK_T_GET_ID | VIRTIO_BLK_T_IN;
adaptExt->vbr.out_hdr.sector = 0;
adaptExt->vbr.out_hdr.ioprio = 0;
adaptExt->vbr.sg[0].physAddr = MmGetPhysicalAddress(&adaptExt->vbr.out_hdr);
adaptExt->vbr.sg[0].length = sizeof(adaptExt->vbr.out_hdr);
adaptExt->vbr.sg[1].physAddr = MmGetPhysicalAddress(&adaptExt->sn);
adaptExt->vbr.sg[1].length = sizeof(adaptExt->sn);
adaptExt->vbr.sg[2].physAddr = MmGetPhysicalAddress(&adaptExt->vbr.status);
adaptExt->vbr.sg[2].length = sizeof(adaptExt->vbr.status);
if (virtqueue_add_buf(adaptExt->vq,
&adaptExt->vbr.sg[0],
1, 2,
&adaptExt->vbr, NULL, 0) >= 0) {
virtqueue_kick(adaptExt->vq);
}
}
/* called when an rx buffer is used, and it's time to digest a message */
static void rpmsg_recv_done(struct virtqueue *rvq)
{
struct rpmsg_hdr *msg;
unsigned int len;
struct rpmsg_endpoint *ept;
struct scatterlist sg;
struct virtproc_info *vrp = rvq->vdev->priv;
struct device *dev = &rvq->vdev->dev;
int err;
msg = virtqueue_get_buf(rvq, &len);
if (!msg) {
dev_err(dev, "uhm, incoming signal, but no used buffer ?\n");
return;
}
dev_dbg(dev, "From: 0x%x, To: 0x%x, Len: %d, Flags: %d, Reserved: %d\n",
msg->src, msg->dst, msg->len,
msg->flags, msg->reserved);
#if 0
print_hex_dump(KERN_DEBUG, "rpmsg_virtio RX: ", DUMP_PREFIX_NONE, 16, 1,
msg, sizeof(*msg) + msg->len, true);
#endif
/*
* We currently use fixed-sized buffers, so trivially sanitize
* the reported payload length.
*/
if (len > RPMSG_BUF_SIZE ||
msg->len > (len - sizeof(struct rpmsg_hdr))) {
dev_warn(dev, "inbound msg too big: (%d, %d)\n", len, msg->len);
return;
}
/* use the dst addr to fetch the callback of the appropriate user */
mutex_lock(&vrp->endpoints_lock);
ept = idr_find(&vrp->endpoints, msg->dst);
mutex_unlock(&vrp->endpoints_lock);
if (ept && ept->cb)
ept->cb(ept->rpdev, msg->data, msg->len, ept->priv, msg->src);
else
dev_warn(dev, "msg received with no recepient\n");
/* publish the real size of the buffer */
sg_init_one(&sg, msg, RPMSG_BUF_SIZE);
/* add the buffer back to the remote processor's virtqueue */
err = virtqueue_add_buf(vrp->rvq, &sg, 0, 1, msg, GFP_KERNEL);
if (err < 0) {
dev_err(dev, "failed to add a virtqueue buffer: %d\n", err);
return;
}
/* tell the remote processor we added another available rx buffer */
virtqueue_kick(vrp->rvq);
}
static void register_buffer(void)
{
struct scatterlist sg;
sg_init_one(&sg, random_data+data_left, RANDOM_DATA_SIZE-data_left);
/* There should always be room for one buffer. */
if (virtqueue_add_buf(vq, &sg, 0, 1, random_data) < 0)
BUG();
virtqueue_kick(vq);
}
/* The host will fill any buffer we give it with sweet, sweet randomness. */
static void register_buffer(u8 *buf, size_t size)
{
struct scatterlist sg;
sg_init_one(&sg, buf, size);
/* There should always be room for one buffer. */
if (virtqueue_add_buf(vq, &sg, 0, 1, buf) < 0)
BUG();
virtqueue_kick(vq);
}
BOOLEAN
SendSRB(
IN PVOID DeviceExtension,
IN PSRB_TYPE Srb
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PSRB_EXTENSION srbExt = SRB_EXTENSION(Srb);
PVOID va = NULL;
ULONGLONG pa = 0;
ULONG QueueNumber = 0;
ULONG OldIrql = 0;
ULONG MessageId = 0;
BOOLEAN kick = FALSE;
STOR_LOCK_HANDLE LockHandle = { 0 };
ULONG status = STOR_STATUS_SUCCESS;
ENTER_FN();
SET_VA_PA();
if (adaptExt->num_queues > 1) {
QueueNumber = adaptExt->cpu_to_vq_map[srbExt->cpu] + VIRTIO_SCSI_REQUEST_QUEUE_0;
MessageId = QueueNumber + 1;
}
else {
QueueNumber = VIRTIO_SCSI_REQUEST_QUEUE_0;
}
VioScsiAcquireSpinLock(DeviceExtension, MessageId, &LockHandle);
if (virtqueue_add_buf(adaptExt->vq[QueueNumber],
&srbExt->sg[0],
srbExt->out, srbExt->in,
&srbExt->cmd, va, pa) >= 0){
kick = TRUE;
}
else {
RhelDbgPrint(TRACE_LEVEL_ERROR, ("%s Can not add packet to queue.\n", __FUNCTION__));
//FIXME
}
VioScsiReleaseSpinLock(DeviceExtension, MessageId, &LockHandle);
if (kick == TRUE) {
virtqueue_kick(adaptExt->vq[QueueNumber]);
}
if (adaptExt->num_queues > 1) {
if (CHECKFLAG(adaptExt->perfFlags, STOR_PERF_OPTIMIZE_FOR_COMPLETION_DURING_STARTIO)) {
// ProcessQueue(DeviceExtension, MessageId, FALSE);
}
}
EXIT_FN();
return kick;
}
size_t VIOSerialSendBuffers(IN PVIOSERIAL_PORT Port,
IN PVOID Buffer,
IN size_t Length)
{
struct virtqueue *vq = GetOutQueue(Port);
struct VirtIOBufferDescriptor sg[QUEUE_DESCRIPTORS];
PVOID buffer = Buffer;
size_t length = Length;
int out = 0;
int ret;
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_WRITE,
"--> %s Buffer: %p Length: %d\n", __FUNCTION__, Buffer, Length);
if (BYTES_TO_PAGES(Length) > QUEUE_DESCRIPTORS)
{
return 0;
}
while (length > 0)
{
sg[out].physAddr = MmGetPhysicalAddress(buffer);
sg[out].length = min(length, PAGE_SIZE);
buffer = (PVOID)((LONG_PTR)buffer + sg[out].length);
length -= sg[out].length;
out += 1;
}
WdfSpinLockAcquire(Port->OutVqLock);
ret = virtqueue_add_buf(vq, sg, out, 0, Buffer, NULL, 0);
virtqueue_kick(vq);
if (ret >= 0)
{
Port->OutVqFull = (ret == 0);
}
else
{
Length = 0;
TraceEvents(TRACE_LEVEL_ERROR, DBG_WRITE,
"Error adding buffer to queue (ret = %d)\n", ret);
}
WdfSpinLockRelease(Port->OutVqLock);
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_WRITE, "<-- %s\n", __FUNCTION__);
return Length;
}
/*
* Send a buffer to the output virtqueue of the given crypto_device.
* If nonblock is false wait until the host acknowledges the data receive.
*/
ssize_t send_buf(struct crypto_device *crdev, void *in_buf, size_t in_count,
bool nonblock)
{
struct scatterlist sg[1];
struct virtqueue *out_vq;
ssize_t ret = 0;
unsigned int len;
debug("Entering\n");
out_vq = crdev->ovq;
/* Discard any consumed buffers. */
reclaim_consumed_buffers(crdev);
sg_init_one(sg, in_buf, in_count);
/* add sg list to virtqueue and notify host */
ret = virtqueue_add_buf(out_vq, sg, 1, 0, in_buf);
if (ret < 0) {
debug("Oops! Error adding buffer to vqueue\n");
in_count = 0;
goto done;
}
if (ret == 0) {
printk(KERN_WARNING "ovq_full!!!!\n");
crdev->ovq_full = true;
}
virtqueue_kick(out_vq);
if (nonblock)
goto done;
/*
* if nonblock is false we wait until the host acknowledges it pushed
* out the data we sent.
*/
while (!virtqueue_get_buf(out_vq, &len))
cpu_relax();
debug("Leaving\n");
done:
/*
* We're expected to return the amount of data we wrote -- all
* of it
*/
return in_count;
}
/*
* Add a buffer to the specified virtqueue.
* Callers should take appropriate locks.
*/
int add_inbuf(struct virtqueue *vq, struct crypto_vq_buffer *buf)
{
struct scatterlist sg[1];
int ret;
debug("Entering\n");
/* Let's make the scatter-gather list. */
sg_init_one(sg, buf->buf, buf->size);
/* Ok now add buf to virtqueue and notify host. */
ret = virtqueue_add_buf(vq, sg, 0, 1, buf);
virtqueue_kick(vq);
debug("Leaving\n");
return ret;
}
VOID
VIOSerialSendCtrlMsg(
IN WDFDEVICE Device,
IN ULONG id,
IN USHORT event,
IN USHORT value
)
{
struct VirtIOBufferDescriptor sg;
struct virtqueue *vq;
UINT len;
PPORTS_DEVICE pContext = GetPortsDevice(Device);
VIRTIO_CONSOLE_CONTROL cpkt;
if (!pContext->isHostMultiport)
{
return;
}
vq = pContext->c_ovq;
TraceEvents(TRACE_LEVEL_INFORMATION, DBG_PNP, "--> %s vq = %p\n", __FUNCTION__, vq);
cpkt.id = id;
cpkt.event = event;
cpkt.value = value;
sg.physAddr = MmGetPhysicalAddress(&cpkt);
sg.length = sizeof(cpkt);
WdfWaitLockAcquire(pContext->COutVqLock, NULL);
if(0 <= virtqueue_add_buf(vq, &sg, 1, 0, &cpkt, NULL, 0))
{
virtqueue_kick(vq);
while(!virtqueue_get_buf(vq, &len))
{
LARGE_INTEGER interval;
interval.QuadPart = -1;
KeDelayExecutionThread(KernelMode, FALSE, &interval);
}
}
WdfWaitLockRelease(pContext->COutVqLock);
TraceEvents(TRACE_LEVEL_INFORMATION, DBG_PNP, "<-- %s\n", __FUNCTION__);
}
BOOLEAN
SynchronizedFlushRoutine(
IN PVOID DeviceExtension,
IN PVOID Context
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PSCSI_REQUEST_BLOCK Srb = (PSCSI_REQUEST_BLOCK) Context;
PRHEL_SRB_EXTENSION srbExt = (PRHEL_SRB_EXTENSION)Srb->SrbExtension;
ULONG fragLen;
PVOID va;
ULONGLONG pa;
SET_VA_PA();
srbExt->vbr.out_hdr.sector = 0;
srbExt->vbr.out_hdr.ioprio = 0;
srbExt->vbr.req = (struct request *)Srb;
srbExt->vbr.out_hdr.type = VIRTIO_BLK_T_FLUSH;
srbExt->out = 1;
srbExt->in = 1;
srbExt->vbr.sg[0].physAddr = ScsiPortGetPhysicalAddress(DeviceExtension, NULL, &srbExt->vbr.out_hdr, &fragLen);
srbExt->vbr.sg[0].length = sizeof(srbExt->vbr.out_hdr);
srbExt->vbr.sg[1].physAddr = ScsiPortGetPhysicalAddress(DeviceExtension, NULL, &srbExt->vbr.status, &fragLen);
srbExt->vbr.sg[1].length = sizeof(srbExt->vbr.status);
if (virtqueue_add_buf(adaptExt->vq,
&srbExt->vbr.sg[0],
srbExt->out, srbExt->in,
&srbExt->vbr, va, pa) >= 0) {
virtqueue_kick(adaptExt->vq);
return TRUE;
}
virtqueue_kick(adaptExt->vq);
#ifdef USE_STORPORT
StorPortBusy(DeviceExtension, 2);
#endif
return FALSE;
}
BOOLEAN
SynchronizedKickEventRoutine(
IN PVOID DeviceExtension,
IN PVOID Context
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PVirtIOSCSIEventNode eventNode = (PVirtIOSCSIEventNode) Context;
PVOID va = NULL;
ULONGLONG pa = 0;
ENTER_FN();
if (virtqueue_add_buf(adaptExt->vq[VIRTIO_SCSI_EVENTS_QUEUE],
&eventNode->sg,
0, 1,
eventNode, va, pa) >= 0){
virtqueue_kick(adaptExt->vq[VIRTIO_SCSI_EVENTS_QUEUE]);
return TRUE;
}
EXIT_ERR();
return FALSE;
}
VOID
RhelGetSerialNumber(
IN PVOID DeviceExtension
)
{
ULONG QueueNumber = 0;
ULONG OldIrql = 0;
ULONG MessageId = 0;
STOR_LOCK_HANDLE LockHandle = { 0 };
struct virtqueue *vq = NULL;
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
QueueNumber = 0;
MessageId = 1;
vq = adaptExt->vq[QueueNumber];
adaptExt->vbr.out_hdr.type = VIRTIO_BLK_T_GET_ID | VIRTIO_BLK_T_IN;
adaptExt->vbr.out_hdr.sector = 0;
adaptExt->vbr.out_hdr.ioprio = 0;
adaptExt->vbr.sg[0].physAddr = MmGetPhysicalAddress(&adaptExt->vbr.out_hdr);
adaptExt->vbr.sg[0].length = sizeof(adaptExt->vbr.out_hdr);
adaptExt->vbr.sg[1].physAddr = MmGetPhysicalAddress(&adaptExt->sn);
adaptExt->vbr.sg[1].length = sizeof(adaptExt->sn);
adaptExt->vbr.sg[2].physAddr = MmGetPhysicalAddress(&adaptExt->vbr.status);
adaptExt->vbr.sg[2].length = sizeof(adaptExt->vbr.status);
// VioStorVQLock(DeviceExtension, MessageId, &LockHandle, FALSE);
if (virtqueue_add_buf(vq,
&adaptExt->vbr.sg[0],
1, 2,
&adaptExt->vbr, NULL, 0) >= 0) {
#ifdef DBG
InterlockedIncrement((LONG volatile*)&adaptExt->inqueue_cnt);
#endif
virtqueue_kick_always(vq);
}
// VioStorVQUnlock(DeviceExtension, MessageId, &LockHandle, FALSE);
}
/*
* Send a control message to the Guest.
*/
ssize_t send_control_msg(struct crypto_device *crdev, unsigned int event,
unsigned int value)
{
struct scatterlist sg[1];
struct virtio_crypto_control cpkt;
struct virtqueue *vq;
unsigned int len;
debug("Entering\n");
cpkt.event = event;
cpkt.value = value;
vq = crdev->c_ovq;
sg_init_one(sg, &cpkt, sizeof(cpkt));
if (virtqueue_add_buf(vq, sg, 1, 0, &cpkt) >= 0) {
virtqueue_kick(vq);
/* Wait until host gets the message. */
while (!virtqueue_get_buf(vq, &len))
cpu_relax();
}
debug("Leaving\n");
return 0;
}
int rpmsg_send_offchannel_raw(struct rpmsg_channel *rpdev, u32 src, u32 dst,
void *data, int len, bool wait)
{
struct virtproc_info *vrp = rpdev->vrp;
struct device *dev = &rpdev->dev;
struct scatterlist sg;
struct rpmsg_hdr *msg;
int err;
if (src == RPMSG_ADDR_ANY || dst == RPMSG_ADDR_ANY) {
dev_err(dev, "invalid addr (src 0x%x, dst 0x%x)\n", src, dst);
return -EINVAL;
}
if (len > RPMSG_BUF_SIZE - sizeof(struct rpmsg_hdr)) {
dev_err(dev, "message is too big (%d)\n", len);
return -EMSGSIZE;
}
msg = get_a_tx_buf(vrp);
if (!msg && !wait)
return -ENOMEM;
while (!msg) {
rpmsg_upref_sleepers(vrp);
err = wait_event_interruptible_timeout(vrp->sendq,
(msg = get_a_tx_buf(vrp)),
msecs_to_jiffies(15000));
rpmsg_downref_sleepers(vrp);
if (!err) {
dev_err(dev, "timeout waiting for a tx buffer\n");
return -ERESTARTSYS;
}
}
msg->len = len;
msg->flags = 0;
msg->src = src;
msg->dst = dst;
msg->reserved = 0;
memcpy(msg->data, data, len);
dev_dbg(dev, "TX From 0x%x, To 0x%x, Len %d, Flags %d, Reserved %d\n",
msg->src, msg->dst, msg->len,
msg->flags, msg->reserved);
print_hex_dump(KERN_DEBUG, "rpmsg_virtio TX: ", DUMP_PREFIX_NONE, 16, 1,
msg, sizeof(*msg) + msg->len, true);
sg_init_one(&sg, msg, sizeof(*msg) + len);
mutex_lock(&vrp->tx_lock);
err = virtqueue_add_buf(vrp->svq, &sg, 1, 0, msg, GFP_KERNEL);
if (err < 0) {
dev_err(dev, "virtqueue_add_buf failed: %d\n", err);
goto out;
}
virtqueue_kick(vrp->svq);
err = 0;
out:
mutex_unlock(&vrp->tx_lock);
return err;
}
static void rpmsg_recv_done(struct virtqueue *rvq)
{
struct rpmsg_hdr *msg;
unsigned int len;
struct rpmsg_endpoint *ept;
struct scatterlist sg;
struct virtproc_info *vrp = rvq->vdev->priv;
struct device *dev = &rvq->vdev->dev;
int err;
msg = virtqueue_get_buf(rvq, &len);
if (!msg) {
dev_err(dev, "uhm, incoming signal, but no used buffer ?\n");
return;
}
dev_dbg(dev, "From: 0x%x, To: 0x%x, Len: %d, Flags: %d, Reserved: %d\n",
msg->src, msg->dst, msg->len,
msg->flags, msg->reserved);
print_hex_dump(KERN_DEBUG, "rpmsg_virtio RX: ", DUMP_PREFIX_NONE, 16, 1,
msg, sizeof(*msg) + msg->len, true);
if (len > RPMSG_BUF_SIZE ||
msg->len > (len - sizeof(struct rpmsg_hdr))) {
dev_warn(dev, "inbound msg too big: (%d, %d)\n", len, msg->len);
return;
}
mutex_lock(&vrp->endpoints_lock);
ept = idr_find(&vrp->endpoints, msg->dst);
/* let's make sure no one deallocates ept while we use it */
if (ept)
kref_get(&ept->refcount);
mutex_unlock(&vrp->endpoints_lock);
if (ept) {
/* make sure ept->cb doesn't go away while we use it */
mutex_lock(&ept->cb_lock);
if (ept->cb)
ept->cb(ept->rpdev, msg->data, msg->len, ept->priv,
msg->src);
mutex_unlock(&ept->cb_lock);
/* farewell, ept, we don't need you anymore */
kref_put(&ept->refcount, __ept_release);
} else
dev_warn(dev, "msg received with no recepient\n");
sg_init_one(&sg, msg, RPMSG_BUF_SIZE);
err = virtqueue_add_buf(vrp->rvq, &sg, 0, 1, msg, GFP_KERNEL);
if (err < 0) {
dev_err(dev, "failed to add a virtqueue buffer: %d\n", err);
return;
}
virtqueue_kick(vrp->rvq);
}
BOOLEAN
RhelDoReadWrite(PVOID DeviceExtension,
PSCSI_REQUEST_BLOCK Srb)
{
PCDB cdb;
ULONG fragLen;
ULONG sgElement;
ULONG BytesLeft;
PVOID DataBuffer;
PADAPTER_EXTENSION adaptExt;
PRHEL_SRB_EXTENSION srbExt;
int num_free;
PVOID va;
ULONGLONG pa;
ULONG i;
ULONG sgMaxElements;
cdb = (PCDB)&Srb->Cdb[0];
srbExt = (PRHEL_SRB_EXTENSION)Srb->SrbExtension;
adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
BytesLeft= Srb->DataTransferLength;
DataBuffer = Srb->DataBuffer;
memset(srbExt, 0, sizeof (RHEL_SRB_EXTENSION));
sgMaxElements = MAX_PHYS_SEGMENTS + 1;
for (i = 0, sgElement = 1; (i < sgMaxElements) && BytesLeft; i++, sgElement++) {
srbExt->vbr.sg[sgElement].physAddr = ScsiPortGetPhysicalAddress(DeviceExtension, Srb, DataBuffer, &fragLen);
srbExt->vbr.sg[sgElement].length = fragLen;
srbExt->Xfer += fragLen;
BytesLeft -= fragLen;
DataBuffer = (PVOID)((ULONG_PTR)DataBuffer + fragLen);
}
srbExt->vbr.out_hdr.sector = RhelGetLba(DeviceExtension, cdb);
srbExt->vbr.out_hdr.ioprio = 0;
srbExt->vbr.req = (struct request *)Srb;
if (Srb->SrbFlags & SRB_FLAGS_DATA_OUT) {
srbExt->vbr.out_hdr.type = VIRTIO_BLK_T_OUT;
srbExt->out = sgElement;
srbExt->in = 1;
} else {
srbExt->vbr.out_hdr.type = VIRTIO_BLK_T_IN;
srbExt->out = 1;
srbExt->in = sgElement;
}
srbExt->vbr.sg[0].physAddr = ScsiPortGetPhysicalAddress(DeviceExtension, NULL, &srbExt->vbr.out_hdr, &fragLen);
srbExt->vbr.sg[0].length = sizeof(srbExt->vbr.out_hdr);
srbExt->vbr.sg[sgElement].physAddr = ScsiPortGetPhysicalAddress(DeviceExtension, NULL, &srbExt->vbr.status, &fragLen);
srbExt->vbr.sg[sgElement].length = sizeof(srbExt->vbr.status);
SET_VA_PA();
num_free = virtqueue_add_buf(adaptExt->vq,
&srbExt->vbr.sg[0],
srbExt->out, srbExt->in,
&srbExt->vbr, va, pa);
if ( num_free >= 0) {
InsertTailList(&adaptExt->list_head, &srbExt->vbr.list_entry);
virtqueue_kick(adaptExt->vq);
srbExt->call_next = FALSE;
if(!adaptExt->indirect && num_free < VIRTIO_MAX_SG) {
srbExt->call_next = TRUE;
} else {
ScsiPortNotification(NextLuRequest, DeviceExtension, Srb->PathId, Srb->TargetId, Srb->Lun);
}
}
return TRUE;
}
/**
* rpmsg_send_offchannel_raw() - send a message across to the remote processor
* @rpdev: the rpmsg channel
* @src: source address
* @dst: destination address
* @data: payload of message
* @len: length of payload
* @wait: indicates whether caller should block in case no TX buffers available
*
* This function is the base implementation for all of the rpmsg sending API.
*
* It will send @data of length @len to @dst, and say it's from @src. The
* message will be sent to the remote processor which the @rpdev channel
* belongs to.
*
* The message is sent using one of the TX buffers that are available for
* communication with this remote processor.
*
* If @wait is true, the caller will be blocked until either a TX buffer is
* available, or 15 seconds elapses (we don't want callers to
* sleep indefinitely due to misbehaving remote processors), and in that
* case -ERESTARTSYS is returned. The number '15' itself was picked
* arbitrarily; there's little point in asking drivers to provide a timeout
* value themselves.
*
* Otherwise, if @wait is false, and there are no TX buffers available,
* the function will immediately fail, and -ENOMEM will be returned.
*
* Normally drivers shouldn't use this function directly; instead, drivers
* should use the appropriate rpmsg_{try}send{to, _offchannel} API
* (see include/linux/rpmsg.h).
*
* Returns 0 on success and an appropriate error value on failure.
*/
int rpmsg_send_offchannel_raw(struct rpmsg_channel *rpdev, u32 src, u32 dst,
void *data, int len, bool wait)
{
struct virtproc_info *vrp = rpdev->vrp;
struct device *dev = &rpdev->dev;
struct scatterlist sg;
struct rpmsg_hdr *msg;
int err;
/* bcasting isn't allowed */
if (src == RPMSG_ADDR_ANY || dst == RPMSG_ADDR_ANY) {
dev_err(dev, "invalid addr (src 0x%x, dst 0x%x)\n", src, dst);
return -EINVAL;
}
/*
* We currently use fixed-sized buffers, and therefore the payload
* length is limited.
*
* One of the possible improvements here is either to support
* user-provided buffers (and then we can also support zero-copy
* messaging), or to improve the buffer allocator, to support
* variable-length buffer sizes.
*/
if (len > RPMSG_BUF_SIZE - sizeof(struct rpmsg_hdr)) {
dev_err(dev, "message is too big (%d)\n", len);
return -EMSGSIZE;
}
/* grab a buffer */
msg = get_a_tx_buf(vrp);
if (!msg && !wait)
return -ENOMEM;
/* no free buffer ? wait for one (but bail after 15 seconds) */
while (!msg) {
/* enable "tx-complete" interrupts, if not already enabled */
rpmsg_upref_sleepers(vrp);
/*
* sleep until a free buffer is available or 15 secs elapse.
* the timeout period is not configurable because there's
* little point in asking drivers to specify that.
* if later this happens to be required, it'd be easy to add.
*/
err = wait_event_interruptible_timeout(vrp->sendq,
(msg = get_a_tx_buf(vrp)),
msecs_to_jiffies(15000));
/* disable "tx-complete" interrupts if we're the last sleeper */
rpmsg_downref_sleepers(vrp);
/* timeout ? */
if (!err) {
dev_err(dev, "timeout waiting for a tx buffer\n");
return -ERESTARTSYS;
}
}
msg->len = len;
msg->flags = 0;
msg->src = src;
msg->dst = dst;
msg->reserved = 0;
memcpy(msg->data, data, len);
dev_dbg(dev, "TX From 0x%x, To 0x%x, Len %d, Flags %d, Reserved %d\n",
msg->src, msg->dst, msg->len,
msg->flags, msg->reserved);
print_hex_dump(KERN_DEBUG, "rpmsg_virtio TX: ", DUMP_PREFIX_NONE, 16, 1,
msg, sizeof(*msg) + msg->len, true);
sg_init_one(&sg, msg, sizeof(*msg) + len);
mutex_lock(&vrp->tx_lock);
/* add message to the remote processor's virtqueue */
err = virtqueue_add_buf(vrp->svq, &sg, 1, 0, msg, GFP_KERNEL);
if (err) {
/*
* need to reclaim the buffer here, otherwise it's lost
* (memory won't leak, but rpmsg won't use it again for TX).
* this will wait for a buffer management overhaul.
*/
dev_err(dev, "virtqueue_add_buf failed: %d\n", err);
goto out;
}
/* tell the remote processor it has a pending message to read */
virtqueue_kick(vrp->svq);
out:
mutex_unlock(&vrp->tx_lock);
return err;
}
static int rpmsg_probe(struct virtio_device *vdev)
{
vq_callback_t *vq_cbs[] = { rpmsg_recv_done, rpmsg_xmit_done };
const char *names[] = { "input", "output" };
struct virtqueue *vqs[2];
struct virtproc_info *vrp;
void *bufs_va;
int err = 0, i;
vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
if (!vrp)
return -ENOMEM;
vrp->vdev = vdev;
idr_init(&vrp->endpoints);
mutex_init(&vrp->endpoints_lock);
mutex_init(&vrp->tx_lock);
init_waitqueue_head(&vrp->sendq);
err = vdev->config->find_vqs(vdev, 2, vqs, vq_cbs, names);
if (err)
goto free_vrp;
vrp->rvq = vqs[0];
vrp->svq = vqs[1];
bufs_va = dma_alloc_coherent(vdev->dev.parent, RPMSG_TOTAL_BUF_SPACE,
&vrp->bufs_dma, GFP_KERNEL);
if (!bufs_va)
goto vqs_del;
dev_dbg(&vdev->dev, "buffers: va %p, dma 0x%llx\n", bufs_va,
(unsigned long long)vrp->bufs_dma);
vrp->rbufs = bufs_va;
vrp->sbufs = bufs_va + RPMSG_TOTAL_BUF_SPACE / 2;
for (i = 0; i < RPMSG_NUM_BUFS / 2; i++) {
struct scatterlist sg;
void *cpu_addr = vrp->rbufs + i * RPMSG_BUF_SIZE;
sg_init_one(&sg, cpu_addr, RPMSG_BUF_SIZE);
err = virtqueue_add_buf(vrp->rvq, &sg, 0, 1, cpu_addr,
GFP_KERNEL);
WARN_ON(err < 0);
}
virtqueue_disable_cb(vrp->svq);
vdev->priv = vrp;
if (virtio_has_feature(vdev, VIRTIO_RPMSG_F_NS)) {
vrp->ns_ept = __rpmsg_create_ept(vrp, NULL, rpmsg_ns_cb,
vrp, RPMSG_NS_ADDR);
if (!vrp->ns_ept) {
dev_err(&vdev->dev, "failed to create the ns ept\n");
err = -ENOMEM;
goto free_coherent;
}
}
virtqueue_kick(vrp->rvq);
dev_info(&vdev->dev, "rpmsg host is online\n");
return 0;
free_coherent:
dma_free_coherent(vdev->dev.parent, RPMSG_TOTAL_BUF_SPACE, bufs_va,
vrp->bufs_dma);
vqs_del:
vdev->config->del_vqs(vrp->vdev);
free_vrp:
kfree(vrp);
return err;
}
BOOLEAN
SendSRB(
IN PVOID DeviceExtension,
IN PSCSI_REQUEST_BLOCK Srb
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PSRB_EXTENSION srbExt = (PSRB_EXTENSION)Srb->SrbExtension;
PVOID va = NULL;
ULONGLONG pa = 0;
ULONG QueueNumber = 0;
ULONG OldIrql = 0;
ULONG MessageId = 0;
BOOLEAN kick = FALSE;
STOR_LOCK_HANDLE LockHandle = { 0 };
ULONG status = STOR_STATUS_SUCCESS;
ENTER_FN();
SET_VA_PA();
RhelDbgPrint(TRACE_LEVEL_INFORMATION, ("Srb %p issued on %d::%d QueueNumber %d\n",
Srb, srbExt->procNum.Group, srbExt->procNum.Number, QueueNumber));
if (adaptExt->num_queues > 1) {
QueueNumber = adaptExt->cpu_to_vq_map[srbExt->procNum.Number];
MessageId = QueueNumber + 1;
if (CHECKFLAG(adaptExt->perfFlags, STOR_PERF_OPTIMIZE_FOR_COMPLETION_DURING_STARTIO)) {
ProcessQueue(DeviceExtension, MessageId, FALSE);
}
// status = StorPortAcquireMSISpinLock(DeviceExtension, MessageId, &OldIrql);
if (status != STOR_STATUS_SUCCESS) {
RhelDbgPrint(TRACE_LEVEL_ERROR, ("%s StorPortAcquireMSISpinLock returned status 0x%x\n", __FUNCTION__, status));
}
}
else {
QueueNumber = VIRTIO_SCSI_REQUEST_QUEUE_0;
StorPortAcquireSpinLock(DeviceExtension, InterruptLock, NULL, &LockHandle);
}
if (virtqueue_add_buf(adaptExt->vq[QueueNumber],
&srbExt->sg[0],
srbExt->out, srbExt->in,
&srbExt->cmd, va, pa) >= 0){
kick = TRUE;
}
else {
RhelDbgPrint(TRACE_LEVEL_WARNING, ("%s Cant add packet to queue.\n", __FUNCTION__));
//FIXME
}
if (adaptExt->num_queues > 1) {
// status = StorPortReleaseMSISpinLock(DeviceExtension, MessageId, OldIrql);
if (status != STOR_STATUS_SUCCESS) {
RhelDbgPrint(TRACE_LEVEL_ERROR, ("%s StorPortReleaseMSISpinLock returned status 0x%x\n", __FUNCTION__, status));
}
}
else {
StorPortReleaseSpinLock(DeviceExtension, &LockHandle);
}
if (kick == TRUE) {
virtqueue_kick(adaptExt->vq[QueueNumber]);
}
return kick;
EXIT_FN();
}
BOOLEAN
RhelDoReadWrite(PVOID DeviceExtension,
PSRB_TYPE Srb)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PSRB_EXTENSION srbExt = SRB_EXTENSION(Srb);
PVOID va = NULL;
ULONGLONG pa = 0ULL;
ULONG QueueNumber = 0;
ULONG OldIrql = 0;
ULONG MessageId = 0;
BOOLEAN result = FALSE;
bool notify = FALSE;
STOR_LOCK_HANDLE LockHandle = { 0 };
ULONG status = STOR_STATUS_SUCCESS;
struct virtqueue *vq = NULL;
SET_VA_PA();
if (adaptExt->num_queues > 1) {
STARTIO_PERFORMANCE_PARAMETERS param;
param.Size = sizeof(STARTIO_PERFORMANCE_PARAMETERS);
status = StorPortGetStartIoPerfParams(DeviceExtension, (PSCSI_REQUEST_BLOCK)Srb, ¶m);
if (status == STOR_STATUS_SUCCESS && param.MessageNumber != 0) {
MessageId = param.MessageNumber;
QueueNumber = MessageId - 1;
RhelDbgPrint(TRACE_LEVEL_INFORMATION, ("%s srb %p, cpu %d :: QueueNumber %lu, MessageNumber %lu, ChannelNumber %lu.\n", __FUNCTION__, Srb, srbExt->cpu, QueueNumber, param.MessageNumber, param.ChannelNumber));
}
else {
RhelDbgPrint(TRACE_LEVEL_ERROR, ("%s StorPortGetStartIoPerfParams failed srb %p cpu %d status 0x%x.\n", __FUNCTION__, Srb, srbExt->cpu, status));
QueueNumber = 0;
MessageId = 1;
}
}
else {
QueueNumber = 0;
MessageId = 1;
}
srbExt->MessageID = MessageId;
vq = adaptExt->vq[QueueNumber];
RhelDbgPrint(TRACE_LEVEL_VERBOSE, ("<--->%s : QueueNumber 0x%x vq = %p\n", __FUNCTION__, QueueNumber, vq));
VioStorVQLock(DeviceExtension, MessageId, &LockHandle, FALSE);
if (virtqueue_add_buf(vq,
&srbExt->vbr.sg[0],
srbExt->out, srbExt->in,
&srbExt->vbr, va, pa) >= 0) {
notify = virtqueue_kick_prepare(vq);
VioStorVQUnlock(DeviceExtension, MessageId, &LockHandle, FALSE);
#ifdef DBG
InterlockedIncrement((LONG volatile*)&adaptExt->inqueue_cnt);
#endif
result = TRUE;
}
else {
VioStorVQUnlock(DeviceExtension, MessageId, &LockHandle, FALSE);
RhelDbgPrint(TRACE_LEVEL_FATAL, ("%s Can not add packet to queue %d.\n", __FUNCTION__, QueueNumber));
StorPortBusy(DeviceExtension, 2);
}
if (notify) {
virtqueue_notify(vq);
}
#if (NTDDI_VERSION > NTDDI_WIN7)
if (adaptExt->num_queues > 1) {
if (CHECKFLAG(adaptExt->perfFlags, STOR_PERF_OPTIMIZE_FOR_COMPLETION_DURING_STARTIO)) {
VioStorCompleteRequest(DeviceExtension, MessageId, FALSE);
}
}
#endif
return result;
}
void vhost_vq_setup(struct vdev_info *dev, struct vq_info *info)
{
struct vhost_vring_state state = { .index = info->idx };
struct vhost_vring_file file = { .index = info->idx };
unsigned long long features = dev->vdev.features[0];
struct vhost_vring_addr addr = {
.index = info->idx,
.desc_user_addr = (uint64_t)(unsigned long)info->vring.desc,
.avail_user_addr = (uint64_t)(unsigned long)info->vring.avail,
.used_user_addr = (uint64_t)(unsigned long)info->vring.used,
};
int r;
r = ioctl(dev->control, VHOST_SET_FEATURES, &features);
assert(r >= 0);
state.num = info->vring.num;
r = ioctl(dev->control, VHOST_SET_VRING_NUM, &state);
assert(r >= 0);
state.num = 0;
r = ioctl(dev->control, VHOST_SET_VRING_BASE, &state);
assert(r >= 0);
r = ioctl(dev->control, VHOST_SET_VRING_ADDR, &addr);
assert(r >= 0);
file.fd = info->kick;
r = ioctl(dev->control, VHOST_SET_VRING_KICK, &file);
assert(r >= 0);
file.fd = info->call;
r = ioctl(dev->control, VHOST_SET_VRING_CALL, &file);
assert(r >= 0);
}
static void vq_info_add(struct vdev_info *dev, int num)
{
struct vq_info *info = &dev->vqs[dev->nvqs];
int r;
info->idx = dev->nvqs;
info->kick = eventfd(0, EFD_NONBLOCK);
info->call = eventfd(0, EFD_NONBLOCK);
r = posix_memalign(&info->ring, 4096, vring_size(num, 4096));
assert(r >= 0);
memset(info->ring, 0, vring_size(num, 4096));
vring_init(&info->vring, num, info->ring, 4096);
info->vq = vring_new_virtqueue(info->vring.num, 4096, &dev->vdev, info->ring,
vq_notify, vq_callback, "test");
assert(info->vq);
info->vq->priv = info;
vhost_vq_setup(dev, info);
dev->fds[info->idx].fd = info->call;
dev->fds[info->idx].events = POLLIN;
dev->nvqs++;
}
static void vdev_info_init(struct vdev_info* dev, unsigned long long features)
{
int r;
memset(dev, 0, sizeof *dev);
dev->vdev.features[0] = features;
dev->vdev.features[1] = features >> 32;
dev->buf_size = 1024;
dev->buf = malloc(dev->buf_size);
assert(dev->buf);
dev->control = open("/dev/vhost-test", O_RDWR);
assert(dev->control >= 0);
r = ioctl(dev->control, VHOST_SET_OWNER, NULL);
assert(r >= 0);
dev->mem = malloc(offsetof(struct vhost_memory, regions) +
sizeof dev->mem->regions[0]);
assert(dev->mem);
memset(dev->mem, 0, offsetof(struct vhost_memory, regions) +
sizeof dev->mem->regions[0]);
dev->mem->nregions = 1;
dev->mem->regions[0].guest_phys_addr = (long)dev->buf;
dev->mem->regions[0].userspace_addr = (long)dev->buf;
dev->mem->regions[0].memory_size = dev->buf_size;
r = ioctl(dev->control, VHOST_SET_MEM_TABLE, dev->mem);
assert(r >= 0);
}
/* TODO: this is pretty bad: we get a cache line bounce
* for the wait queue on poll and another one on read,
* plus the read which is there just to clear the
* current state. */
static void wait_for_interrupt(struct vdev_info *dev)
{
int i;
unsigned long long val;
poll(dev->fds, dev->nvqs, -1);
for (i = 0; i < dev->nvqs; ++i)
if (dev->fds[i].revents & POLLIN) {
read(dev->fds[i].fd, &val, sizeof val);
}
}
static void run_test(struct vdev_info *dev, struct vq_info *vq, int bufs)
{
struct scatterlist sl;
long started = 0, completed = 0;
long completed_before;
int r, test = 1;
unsigned len;
long long spurious = 0;
r = ioctl(dev->control, VHOST_TEST_RUN, &test);
assert(r >= 0);
for (;;) {
virtqueue_disable_cb(vq->vq);
completed_before = completed;
do {
if (started < bufs) {
sg_init_one(&sl, dev->buf, dev->buf_size);
r = virtqueue_add_buf(vq->vq, &sl, 1, 0,
dev->buf + started);
if (likely(r >= 0)) {
++started;
virtqueue_kick(vq->vq);
}
} else
r = -1;
/* Flush out completed bufs if any */
if (virtqueue_get_buf(vq->vq, &len)) {
++completed;
r = 0;
}
} while (r >= 0);
if (completed == completed_before)
++spurious;
assert(completed <= bufs);
assert(started <= bufs);
if (completed == bufs)
break;
if (virtqueue_enable_cb(vq->vq)) {
wait_for_interrupt(dev);
}
}
test = 0;
r = ioctl(dev->control, VHOST_TEST_RUN, &test);
assert(r >= 0);
fprintf(stderr, "spurious wakeus: 0x%llx\n", spurious);
}
const char optstring[] = "h";
const struct option longopts[] = {
{
.name = "help",
.val = 'h',
},
{
.name = "event-idx",
BOOLEAN
RhelDoFlush(
PVOID DeviceExtension,
PSRB_TYPE Srb,
BOOLEAN resend,
BOOLEAN bIsr
)
{
PADAPTER_EXTENSION adaptExt = (PADAPTER_EXTENSION)DeviceExtension;
PSRB_EXTENSION srbExt = SRB_EXTENSION(Srb);
ULONG fragLen = 0UL;
PVOID va = NULL;
ULONGLONG pa = 0ULL;
ULONG QueueNumber = 0;
ULONG OldIrql = 0;
ULONG MessageId = 0;
BOOLEAN result = FALSE;
bool notify = FALSE;
STOR_LOCK_HANDLE LockHandle = { 0 };
ULONG status = STOR_STATUS_SUCCESS;
struct virtqueue *vq = NULL;
SET_VA_PA();
if (resend) {
MessageId = srbExt->MessageID;
QueueNumber = MessageId - 1;
} else if (adaptExt->num_queues > 1) {
STARTIO_PERFORMANCE_PARAMETERS param;
param.Size = sizeof(STARTIO_PERFORMANCE_PARAMETERS);
status = StorPortGetStartIoPerfParams(DeviceExtension, (PSCSI_REQUEST_BLOCK)Srb, ¶m);
if (status == STOR_STATUS_SUCCESS && param.MessageNumber != 0) {
MessageId = param.MessageNumber;
QueueNumber = MessageId - 1;
RhelDbgPrint(TRACE_LEVEL_INFORMATION, ("%s srb %p, cpu %d :: QueueNumber %lu, MessageNumber %lu, ChannelNumber %lu.\n", __FUNCTION__, Srb, srbExt->cpu, QueueNumber, param.MessageNumber, param.ChannelNumber));
}
else {
RhelDbgPrint(TRACE_LEVEL_ERROR, ("%s StorPortGetStartIoPerfParams failed. srb %p cpu %d status 0x%x.\n",__FUNCTION__, Srb, srbExt->cpu, status));
QueueNumber = 0;
MessageId = 1;
}
} else {
QueueNumber = 0;
MessageId = 1;
}
srbExt->MessageID = MessageId;
vq = adaptExt->vq[QueueNumber];
srbExt->vbr.out_hdr.sector = 0;
srbExt->vbr.out_hdr.ioprio = 0;
srbExt->vbr.req = (struct request *)Srb;
srbExt->vbr.out_hdr.type = VIRTIO_BLK_T_FLUSH;
srbExt->out = 1;
srbExt->in = 1;
srbExt->vbr.sg[0].physAddr = StorPortGetPhysicalAddress(DeviceExtension, NULL, &srbExt->vbr.out_hdr, &fragLen);
srbExt->vbr.sg[0].length = sizeof(srbExt->vbr.out_hdr);
srbExt->vbr.sg[1].physAddr = StorPortGetPhysicalAddress(DeviceExtension, NULL, &srbExt->vbr.status, &fragLen);
srbExt->vbr.sg[1].length = sizeof(srbExt->vbr.status);
VioStorVQLock(DeviceExtension, MessageId, &LockHandle, FALSE);
if (virtqueue_add_buf(vq,
&srbExt->vbr.sg[0],
srbExt->out, srbExt->in,
&srbExt->vbr, va, pa) >= 0) {
notify = virtqueue_kick_prepare(vq);
VioStorVQUnlock(DeviceExtension, MessageId, &LockHandle, FALSE);
result = TRUE;
#ifdef DBG
InterlockedIncrement((LONG volatile*)&adaptExt->inqueue_cnt);
#endif
}
else {
VioStorVQUnlock(DeviceExtension, MessageId, &LockHandle, FALSE);
RhelDbgPrint(TRACE_LEVEL_FATAL, ("%s Can not add packet to queue %d.\n", __FUNCTION__, QueueNumber));
StorPortBusy(DeviceExtension, 2);
}
if (notify) {
virtqueue_notify(vq);
}
return result;
}
static int rpmsg_probe(struct virtio_device *vdev)
{
vq_callback_t *vq_cbs[] = { rpmsg_recv_done, rpmsg_xmit_done };
const char *names[] = { "input", "output" };
struct virtqueue *vqs[2];
struct virtproc_info *vrp;
void *bufs_va;
int err = 0, i, vproc_id;
vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
if (!vrp)
return -ENOMEM;
vrp->vdev = vdev;
idr_init(&vrp->endpoints);
mutex_init(&vrp->endpoints_lock);
mutex_init(&vrp->tx_lock);
init_waitqueue_head(&vrp->sendq);
if (!idr_pre_get(&vprocs, GFP_KERNEL))
goto free_vrp;
mutex_lock(&vprocs_mutex);
err = idr_get_new(&vprocs, vrp, &vproc_id);
mutex_unlock(&vprocs_mutex);
if (err) {
dev_err(&vdev->dev, "idr_get_new failed: %d\n", err);
goto free_vrp;
}
vrp->id = vproc_id;
/* We expect two virtqueues, rx and tx (and in this order) */
err = vdev->config->find_vqs(vdev, 2, vqs, vq_cbs, names);
if (err)
goto rem_idr;
vrp->rvq = vqs[0];
vrp->svq = vqs[1];
/* allocate coherent memory for the buffers */
bufs_va = dma_alloc_coherent(vdev->dev.parent->parent,
RPMSG_TOTAL_BUF_SPACE,
&vrp->bufs_dma, GFP_KERNEL);
if (!bufs_va)
goto vqs_del;
dev_dbg(&vdev->dev, "buffers: va %p, dma 0x%llx\n", bufs_va,
(unsigned long long)vrp->bufs_dma);
/* half of the buffers is dedicated for RX */
vrp->rbufs = bufs_va;
/* and half is dedicated for TX */
vrp->sbufs = bufs_va + RPMSG_TOTAL_BUF_SPACE / 2;
/* set up the receive buffers */
for (i = 0; i < RPMSG_NUM_BUFS / 2; i++) {
struct scatterlist sg;
void *cpu_addr = vrp->rbufs + i * RPMSG_BUF_SIZE;
sg_init_one(&sg, cpu_addr, RPMSG_BUF_SIZE);
err = virtqueue_add_buf(vrp->rvq, &sg, 0, 1, cpu_addr,
GFP_KERNEL);
WARN_ON(err); /* sanity check; this can't really happen */
}
/* suppress "tx-complete" interrupts */
virtqueue_disable_cb(vrp->svq);
vdev->priv = vrp;
/* if supported by the remote processor, enable the name service */
if (virtio_has_feature(vdev, VIRTIO_RPMSG_F_NS)) {
/* a dedicated endpoint handles the name service msgs */
vrp->ns_ept = __rpmsg_create_ept(vrp, NULL, rpmsg_ns_cb,
vrp, RPMSG_NS_ADDR);
if (!vrp->ns_ept) {
dev_err(&vdev->dev, "failed to create the ns ept\n");
err = -ENOMEM;
goto free_coherent;
}
}
/* tell the remote processor it can start sending messages */
virtqueue_kick(vrp->rvq);
dev_info(&vdev->dev, "rpmsg host is online\n");
return 0;
free_coherent:
dma_free_coherent(vdev->dev.parent->parent, RPMSG_TOTAL_BUF_SPACE,
bufs_va, vrp->bufs_dma);
vqs_del:
vdev->config->del_vqs(vrp->vdev);
rem_idr:
mutex_lock(&vprocs_mutex);
idr_remove(&vprocs, vproc_id);
mutex_unlock(&vprocs_mutex);
free_vrp:
kfree(vrp);
return err;
}
static NTSTATUS VirtQueueAddBuffer(IN PDEVICE_CONTEXT Context,
IN WDFREQUEST Request,
IN size_t Length)
{
PREAD_BUFFER_ENTRY entry;
size_t length;
struct virtqueue *vq = Context->VirtQueue;
struct VirtIOBufferDescriptor sg;
int ret;
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_READ, "--> %!FUNC!");
entry = (PREAD_BUFFER_ENTRY)ExAllocatePoolWithTag(NonPagedPool,
sizeof(READ_BUFFER_ENTRY), VIRT_RNG_MEMORY_TAG);
if (entry == NULL)
{
TraceEvents(TRACE_LEVEL_ERROR, DBG_READ,
"Failed to allocate a read entry.");
return STATUS_INSUFFICIENT_RESOURCES;
}
length = min(Length, PAGE_SIZE);
entry->Buffer = ExAllocatePoolWithTag(NonPagedPool, length,
VIRT_RNG_MEMORY_TAG);
if (entry->Buffer == NULL)
{
TraceEvents(TRACE_LEVEL_ERROR, DBG_READ,
"Failed to allocate a read buffer.");
ExFreePoolWithTag(entry, VIRT_RNG_MEMORY_TAG);
return STATUS_INSUFFICIENT_RESOURCES;
}
entry->Request = Request;
sg.physAddr = MmGetPhysicalAddress(entry->Buffer);
sg.length = (unsigned)length;
WdfSpinLockAcquire(Context->VirtQueueLock);
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_READ,
"Push %p Request: %p Buffer: %p",
entry, entry->Request, entry->Buffer);
PushEntryList(&Context->ReadBuffersList, &entry->ListEntry);
ret = virtqueue_add_buf(vq, &sg, 0, 1, entry, NULL, 0);
if (ret < 0)
{
PSINGLE_LIST_ENTRY removed;
TraceEvents(TRACE_LEVEL_ERROR, DBG_READ,
"Failed to add buffer to virt queue.");
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_READ,
"Pop %p Request: %p Buffer: %p",
entry, entry->Request, entry->Buffer);
removed = PopEntryList(&Context->ReadBuffersList);
NT_ASSERT(entry == CONTAINING_RECORD(
removed, READ_BUFFER_ENTRY, ListEntry));
ExFreePoolWithTag(entry->Buffer, VIRT_RNG_MEMORY_TAG);
ExFreePoolWithTag(entry, VIRT_RNG_MEMORY_TAG);
WdfSpinLockRelease(Context->VirtQueueLock);
return STATUS_UNSUCCESSFUL;
}
WdfSpinLockRelease(Context->VirtQueueLock);
virtqueue_kick(vq);
TraceEvents(TRACE_LEVEL_VERBOSE, DBG_READ, "<-- %!FUNC!");
return STATUS_SUCCESS;
}
static int virtio_hwkey_probe(struct virtio_device *vdev)
{
int ret = 0;
vqidx = 0;
printk(KERN_INFO "virtio hwkey driver is probed\n");
/* init virtio */
vdev->priv = vh = kmalloc(sizeof(*vh), GFP_KERNEL);
if (!vh) {
return -ENOMEM;
}
memset(&vh->vbuf, 0x00, sizeof(vh->vbuf));
vh->vdev = vdev;
vh->vq = virtio_find_single_vq(vh->vdev, vq_hwkey_callback, "virtio-hwkey-vq");
if (IS_ERR(vh->vq)) {
ret = PTR_ERR(vh->vq);
kfree(vh);
vdev->priv = NULL;
return ret;
}
/* enable callback */
virtqueue_enable_cb(vh->vq);
sg_init_table(vh->sg, MAX_BUF_COUNT);
/* prepare the buffers */
for (index = 0; index < MAX_BUF_COUNT; index++) {
sg_set_buf(&vh->sg[index], &vh->vbuf[index], sizeof(EmulHwkeyEvent));
if (err < 0) {
printk(KERN_ERR "failed to add buffer\n");
kfree(vh);
vdev->priv = NULL;
return ret;
}
}
err = virtqueue_add_buf(vh->vq, vh->sg, 0,
MAX_BUF_COUNT, (void *)MAX_BUF_COUNT, GFP_ATOMIC);
/* register for input device */
vh->idev = input_allocate_device();
if (!vh->idev) {
printk(KERN_ERR "failed to allocate a input hwkey device\n");
ret = -1;
kfree(vh);
vdev->priv = NULL;
return ret;
}
vh->idev->name = "Maru Virtio Hwkey";
vh->idev->dev.parent = &(vdev->dev);
input_set_drvdata(vh->idev, vh);
vh->idev->open = input_hwkey_open;
vh->idev->close = input_hwkey_close;
vh->idev->evbit[0] = BIT_MASK(EV_KEY);
/* to support any keycode */
memset(vh->idev->keybit, 0xffffffff, sizeof(unsigned long) * BITS_TO_LONGS(KEY_CNT));
ret = input_register_device(vh->idev);
if (ret) {
printk(KERN_ERR "input hwkey driver cannot registered\n");
ret = -1;
input_free_device(vh->idev);
kfree(vh);
vdev->priv = NULL;
return ret;
}
virtqueue_kick(vh->vq);
index = 0;
return 0;
}