1 // Copyright 2018 The Fuchsia Authors. All rights reserved.
2 // Use of this source code is governed by a BSD-style license that can be
3 // found in the LICENSE file.
4
5 #include <ddk/binding.h>
6 #include <ddk/device.h>
7 #include <ddk/driver.h>
8
9 #include <zircon/syscalls.h>
10 #include <zircon/types.h>
11
12 #include <stdio.h>
13 #include <stdlib.h>
14 #include <string.h>
15 #include <threads.h>
16
17 // fifo must be a power of 2 for the math to work
18 #define FIFOSIZE 32768
19 #define FIFOMASK (FIFOSIZE - 1)
20
21 typedef struct {
22 zx_device_t* zxdev;
23 mtx_t lock;
24 uint32_t head;
25 uint32_t tail;
26 char data[FIFOSIZE];
27 } fifodev_t;
28
fifo_readable(fifodev_t * fifo)29 static size_t fifo_readable(fifodev_t* fifo) {
30 return (fifo->head - fifo->tail) & FIFOMASK;
31 }
32
fifo_writable(fifodev_t * fifo)33 static size_t fifo_writable(fifodev_t* fifo) {
34 return FIFOMASK - ((fifo->head - fifo->tail) & FIFOMASK);
35 }
36
fifo_put(fifodev_t * fifo,const void * buf,size_t len)37 static size_t fifo_put(fifodev_t* fifo, const void* buf, size_t len) {
38 size_t count = fifo_writable(fifo);
39 uint32_t pos = fifo->head & FIFOMASK;
40 size_t space = FIFOSIZE - pos;
41 if (count > space) { // don't wrap around (single copy)
42 count = space;
43 }
44 if (count > len) { // limit to requested count
45 count = len;
46 }
47 memcpy(fifo->data + pos, buf, count);
48 fifo->head += count;
49 return count;
50 }
51
fifo_get(fifodev_t * fifo,void * buf,size_t len)52 static size_t fifo_get(fifodev_t* fifo, void* buf, size_t len) {
53 size_t count = fifo_readable(fifo);
54 uint32_t pos = fifo->tail & FIFOMASK;
55 size_t space = FIFOSIZE - pos;
56 if (count > space) { // don't wrap around (single copy)
57 count = space;
58 }
59 if (count > len) { // limit to requested count
60 count = len;
61 }
62 memcpy(buf, fifo->data + pos, count);
63 fifo->tail += count;
64 return count;
65 }
66
67
fifo_read(void * ctx,void * buf,size_t len,zx_off_t off,size_t * actual)68 static zx_status_t fifo_read(void* ctx, void* buf, size_t len,
69 zx_off_t off, size_t* actual) {
70 fifodev_t* fifo = ctx;
71
72 mtx_lock(&fifo->lock);
73 size_t n = 0;
74 size_t count;
75 while ((count = fifo_get(fifo, buf, len)) > 0) {
76 len -= count;
77 buf += count;
78 n += count;
79 }
80 if (n == 0) {
81 device_state_clr(fifo->zxdev, DEV_STATE_READABLE);
82 } else {
83 device_state_set(fifo->zxdev, DEV_STATE_WRITABLE);
84 }
85 mtx_unlock(&fifo->lock);
86 *actual = n;
87
88 return (n == 0) ? ZX_ERR_SHOULD_WAIT : ZX_OK;
89 }
90
fifo_write(void * ctx,const void * buf,size_t len,zx_off_t off,size_t * actual)91 static zx_status_t fifo_write(void* ctx, const void* buf, size_t len,
92 zx_off_t off, size_t* actual) {
93
94 fifodev_t* fifo = ctx;
95
96 mtx_lock(&fifo->lock);
97 size_t n = 0;
98 size_t count;
99 while ((count = fifo_put(fifo, buf, len)) > 0) {
100 len -= count;
101 buf += count;
102 n += count;
103 }
104 if (n == 0) {
105 device_state_clr(fifo->zxdev, DEV_STATE_WRITABLE);
106 } else {
107 device_state_set(fifo->zxdev, DEV_STATE_READABLE);
108 }
109 mtx_unlock(&fifo->lock);
110 *actual = n;
111
112 return (n == 0) ? ZX_ERR_SHOULD_WAIT : ZX_OK;
113 }
114
fifo_release(void * ctx)115 static void fifo_release(void* ctx) {
116 fifodev_t* fifo = ctx;
117 free(fifo);
118 }
119
120 static zx_protocol_device_t fifo_ops = {
121 .version = DEVICE_OPS_VERSION,
122 .read = fifo_read,
123 .write = fifo_write,
124 .release = fifo_release,
125 };
126
fifo_bind(void * ctx,zx_device_t * parent)127 static zx_status_t fifo_bind(void* ctx, zx_device_t* parent) {
128 fifodev_t* fifo = calloc(1, sizeof(fifodev_t));
129 if (fifo == NULL) {
130 return ZX_ERR_NO_MEMORY;
131 }
132 mtx_init(&fifo->lock, mtx_plain);
133
134 device_add_args_t args = {
135 .version = DEVICE_ADD_ARGS_VERSION,
136 .name = "demo-fifo",
137 .ctx = fifo,
138 .ops = &fifo_ops,
139 };
140 zx_status_t status = device_add(parent, &args, &fifo->zxdev);
141 if (status != ZX_OK) {
142 free(fifo);
143 return status;
144 }
145
146 // initially we're empty, so writable but not readable
147 device_state_set(fifo->zxdev, DEV_STATE_WRITABLE);
148
149 return ZX_OK;
150 }
151
152 static zx_driver_ops_t fifo_driver_ops = {
153 .version = DRIVER_OPS_VERSION,
154 .bind = fifo_bind,
155 };
156
157 ZIRCON_DRIVER_BEGIN(demo_fifo, fifo_driver_ops, "zircon", "0.1", 1)
158 BI_MATCH_IF(EQ, BIND_PROTOCOL, ZX_PROTOCOL_MISC_PARENT),
159 ZIRCON_DRIVER_END(demo_fifo)
160