1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22#include <linux/slab.h>
23#include <linux/cdev.h>
24#include <linux/module.h>
25#include <linux/fs.h>
26#include <linux/device.h>
27#include <linux/sched.h>
28#include <linux/mutex.h>
29#include <linux/delay.h>
30#include <linux/uaccess.h>
31#include <linux/workqueue.h>
32#include <linux/poll.h>
33
34#include <mach/msm_smd.h>
35
36#define NUM_SMD_PKT_PORTS 9
37#define DEVICE_NAME "smdpkt"
38#define MAX_BUF_SIZE 2048
39
40struct smd_pkt_dev {
41 struct cdev cdev;
42 struct device *devicep;
43
44 struct smd_channel *ch;
45 int open_count;
46 struct mutex ch_lock;
47 struct mutex rx_lock;
48 struct mutex tx_lock;
49 wait_queue_head_t ch_read_wait_queue;
50 wait_queue_head_t ch_opened_wait_queue;
51
52 int i;
53
54 unsigned char tx_buf[MAX_BUF_SIZE];
55 unsigned char rx_buf[MAX_BUF_SIZE];
56 int remote_open;
57
58} *smd_pkt_devp[NUM_SMD_PKT_PORTS];
59
60struct class *smd_pkt_classp;
61static dev_t smd_pkt_number;
62
63static int msm_smd_pkt_debug_enable;
64module_param_named(debug_enable, msm_smd_pkt_debug_enable,
65 int, S_IRUGO | S_IWUSR | S_IWGRP);
66
67#ifdef DEBUG
68#define D_DUMP_BUFFER(prestr, cnt, buf) do { \
69 int i; \
70 if (msm_smd_pkt_debug_enable) { \
71 pr_debug("%s", prestr); \
72 for (i = 0; i < cnt; i++) \
73 pr_debug("%.2x", buf[i]); \
74 pr_debug("\n"); \
75 } \
76 } while (0)
77#else
78#define D_DUMP_BUFFER(prestr, cnt, buf) do {} while (0)
79#endif
80
81#ifdef DEBUG
82#define DBG(x...) do { \
83 if (msm_smd_pkt_debug_enable) \
84 pr_debug(x); \
85 } while (0)
86#else
87#define DBG(x...) do {} while (0)
88#endif
89
90static void check_and_wakeup_reader(struct smd_pkt_dev *smd_pkt_devp)
91{
92 int sz;
93
94 if (!smd_pkt_devp || !smd_pkt_devp->ch)
95 return;
96
97 sz = smd_cur_packet_size(smd_pkt_devp->ch);
98 if (sz == 0) {
99 DBG("no packet\n");
100 return;
101 }
102 if (sz > smd_read_avail(smd_pkt_devp->ch)) {
103 DBG("incomplete packet\n");
104 return;
105 }
106
107 DBG("waking up reader\n");
108 wake_up_interruptible(&smd_pkt_devp->ch_read_wait_queue);
109}
110
111static int smd_pkt_read(struct file *file, char __user *buf,
112 size_t count, loff_t *ppos)
113{
114 int r, bytes_read;
115 struct smd_pkt_dev *smd_pkt_devp;
116 struct smd_channel *chl;
117
118 DBG("read %d bytes\n", count);
119 if (count > MAX_BUF_SIZE)
120 return -EINVAL;
121
122 smd_pkt_devp = file->private_data;
123 if (!smd_pkt_devp || !smd_pkt_devp->ch)
124 return -EINVAL;
125
126 chl = smd_pkt_devp->ch;
127wait_for_packet:
128 r = wait_event_interruptible(smd_pkt_devp->ch_read_wait_queue,
129 (smd_cur_packet_size(chl) > 0 &&
130 smd_read_avail(chl) >=
131 smd_cur_packet_size(chl)));
132
133 if (r < 0) {
134 if (r != -ERESTARTSYS)
135 pr_err("wait returned %d\n", r);
136 return r;
137 }
138
139 mutex_lock(&smd_pkt_devp->rx_lock);
140
141 bytes_read = smd_cur_packet_size(smd_pkt_devp->ch);
142 if (bytes_read == 0 ||
143 bytes_read < smd_read_avail(smd_pkt_devp->ch)) {
144 mutex_unlock(&smd_pkt_devp->rx_lock);
145 DBG("Nothing to read\n");
146 goto wait_for_packet;
147 }
148
149 if (bytes_read > count) {
150 mutex_unlock(&smd_pkt_devp->rx_lock);
151 pr_info("packet size %d > buffer size %d", bytes_read, count);
152 return -EINVAL;
153 }
154
155 r = smd_read(smd_pkt_devp->ch, smd_pkt_devp->rx_buf, bytes_read);
156 if (r != bytes_read) {
157 mutex_unlock(&smd_pkt_devp->rx_lock);
158 pr_err("smd_read failed to read %d bytes: %d\n", bytes_read, r);
159 return -EIO;
160 }
161
162 D_DUMP_BUFFER("read: ", bytes_read, smd_pkt_devp->rx_buf);
163 r = copy_to_user(buf, smd_pkt_devp->rx_buf, bytes_read);
164 mutex_unlock(&smd_pkt_devp->rx_lock);
165 if (r) {
166 pr_err("copy_to_user failed %d\n", r);
167 return -EFAULT;
168 }
169
170 DBG("read complete %d bytes\n", bytes_read);
171 check_and_wakeup_reader(smd_pkt_devp);
172
173 return bytes_read;
174}
175
176static int smd_pkt_write(struct file *file, const char __user *buf,
177 size_t count, loff_t *ppos)
178{
179 int r;
180 struct smd_pkt_dev *smd_pkt_devp;
181
182 if (count > MAX_BUF_SIZE)
183 return -EINVAL;
184
185 DBG("writting %d bytes\n", count);
186
187 smd_pkt_devp = file->private_data;
188 if (!smd_pkt_devp || !smd_pkt_devp->ch)
189 return -EINVAL;
190
191 mutex_lock(&smd_pkt_devp->tx_lock);
192 if (smd_write_avail(smd_pkt_devp->ch) < count) {
193 mutex_unlock(&smd_pkt_devp->tx_lock);
194 DBG("Not enough space to write\n");
195 return -ENOMEM;
196 }
197
198 D_DUMP_BUFFER("write: ", count, buf);
199 r = copy_from_user(smd_pkt_devp->tx_buf, buf, count);
200 if (r) {
201 mutex_unlock(&smd_pkt_devp->tx_lock);
202 pr_err("copy_from_user failed %d\n", r);
203 return -EFAULT;
204 }
205
206 r = smd_write(smd_pkt_devp->ch, smd_pkt_devp->tx_buf, count);
207 if (r != count) {
208 mutex_unlock(&smd_pkt_devp->tx_lock);
209 pr_err("smd_write failed to write %d bytes: %d.\n", count, r);
210 return -EIO;
211 }
212 mutex_unlock(&smd_pkt_devp->tx_lock);
213
214 DBG("wrote %d bytes\n", count);
215 return count;
216}
217
218static unsigned int smd_pkt_poll(struct file *file, poll_table *wait)
219{
220 struct smd_pkt_dev *smd_pkt_devp;
221 unsigned int mask = 0;
222
223 smd_pkt_devp = file->private_data;
224 if (!smd_pkt_devp)
225 return POLLERR;
226
227 DBG("poll waiting\n");
228 poll_wait(file, &smd_pkt_devp->ch_read_wait_queue, wait);
229 if (smd_read_avail(smd_pkt_devp->ch))
230 mask |= POLLIN | POLLRDNORM;
231
232 DBG("poll return\n");
233 return mask;
234}
235
236static void smd_pkt_ch_notify(void *priv, unsigned event)
237{
238 struct smd_pkt_dev *smd_pkt_devp = priv;
239
240 if (smd_pkt_devp->ch == 0)
241 return;
242
243 switch (event) {
244 case SMD_EVENT_DATA:
245 DBG("data\n");
246 check_and_wakeup_reader(smd_pkt_devp);
247 break;
248
249 case SMD_EVENT_OPEN:
250 DBG("remote open\n");
251 smd_pkt_devp->remote_open = 1;
252 wake_up_interruptible(&smd_pkt_devp->ch_opened_wait_queue);
253 break;
254
255 case SMD_EVENT_CLOSE:
256 smd_pkt_devp->remote_open = 0;
257 pr_info("remote closed\n");
258 break;
259
260 default:
261 pr_err("unknown event %d\n", event);
262 break;
263 }
264}
265
266static char *smd_pkt_dev_name[] = {
267 "smdcntl0",
268 "smdcntl1",
269 "smdcntl2",
270 "smdcntl3",
271 "smdcntl4",
272 "smdcntl5",
273 "smdcntl6",
274 "smdcntl7",
275 "smd22",
276};
277
278static char *smd_ch_name[] = {
279 "DATA5_CNTL",
280 "DATA6_CNTL",
281 "DATA7_CNTL",
282 "DATA8_CNTL",
283 "DATA9_CNTL",
284 "DATA12_CNTL",
285 "DATA13_CNTL",
286 "DATA14_CNTL",
287 "DATA22",
288};
289
290static int smd_pkt_open(struct inode *inode, struct file *file)
291{
292 int r = 0;
293 struct smd_pkt_dev *smd_pkt_devp;
294
295 smd_pkt_devp = container_of(inode->i_cdev, struct smd_pkt_dev, cdev);
296 if (!smd_pkt_devp)
297 return -EINVAL;
298
299 file->private_data = smd_pkt_devp;
300
301 mutex_lock(&smd_pkt_devp->ch_lock);
302 if (smd_pkt_devp->open_count == 0) {
303 r = smd_open(smd_ch_name[smd_pkt_devp->i],
304 &smd_pkt_devp->ch, smd_pkt_devp,
305 smd_pkt_ch_notify);
306 if (r < 0) {
307 pr_err("smd_open failed for %s, %d\n",
308 smd_ch_name[smd_pkt_devp->i], r);
309 goto out;
310 }
311
312 r = wait_event_interruptible_timeout(
313 smd_pkt_devp->ch_opened_wait_queue,
314 smd_pkt_devp->remote_open,
315 msecs_to_jiffies(2 * HZ));
316 if (r == 0)
317 r = -ETIMEDOUT;
318
319 if (r < 0) {
320 pr_err("wait returned %d\n", r);
321 smd_close(smd_pkt_devp->ch);
322 smd_pkt_devp->ch = 0;
323 } else {
324 smd_pkt_devp->open_count++;
325 r = 0;
326 }
327 }
328out:
329 mutex_unlock(&smd_pkt_devp->ch_lock);
330 return r;
331}
332
333static int smd_pkt_release(struct inode *inode, struct file *file)
334{
335 int r = 0;
336 struct smd_pkt_dev *smd_pkt_devp = file->private_data;
337
338 if (!smd_pkt_devp)
339 return -EINVAL;
340
341 mutex_lock(&smd_pkt_devp->ch_lock);
342 if (--smd_pkt_devp->open_count == 0) {
343 r = smd_close(smd_pkt_devp->ch);
344 smd_pkt_devp->ch = 0;
345 }
346 mutex_unlock(&smd_pkt_devp->ch_lock);
347
348 return r;
349}
350
351static const struct file_operations smd_pkt_fops = {
352 .owner = THIS_MODULE,
353 .open = smd_pkt_open,
354 .release = smd_pkt_release,
355 .read = smd_pkt_read,
356 .write = smd_pkt_write,
357 .poll = smd_pkt_poll,
358};
359
360static int __init smd_pkt_init(void)
361{
362 int i;
363 int r;
364
365 r = alloc_chrdev_region(&smd_pkt_number, 0,
366 NUM_SMD_PKT_PORTS, DEVICE_NAME);
367 if (r) {
368 pr_err("alloc_chrdev_region() failed %d\n", r);
369 return r;
370 }
371
372 smd_pkt_classp = class_create(THIS_MODULE, DEVICE_NAME);
373 if (IS_ERR(smd_pkt_classp)) {
374 r = PTR_ERR(smd_pkt_classp);
375 pr_err("class_create() failed %d\n", r);
376 goto unreg_chardev;
377 }
378
379 for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
380 smd_pkt_devp[i] = kzalloc(sizeof(struct smd_pkt_dev),
381 GFP_KERNEL);
382 if (!smd_pkt_devp[i]) {
383 pr_err("kmalloc() failed\n");
384 goto clean_cdevs;
385 }
386
387 smd_pkt_devp[i]->i = i;
388
389 init_waitqueue_head(&smd_pkt_devp[i]->ch_read_wait_queue);
390 smd_pkt_devp[i]->remote_open = 0;
391 init_waitqueue_head(&smd_pkt_devp[i]->ch_opened_wait_queue);
392
393 mutex_init(&smd_pkt_devp[i]->ch_lock);
394 mutex_init(&smd_pkt_devp[i]->rx_lock);
395 mutex_init(&smd_pkt_devp[i]->tx_lock);
396
397 cdev_init(&smd_pkt_devp[i]->cdev, &smd_pkt_fops);
398 smd_pkt_devp[i]->cdev.owner = THIS_MODULE;
399
400 r = cdev_add(&smd_pkt_devp[i]->cdev,
401 (smd_pkt_number + i), 1);
402 if (r) {
403 pr_err("cdev_add() failed %d\n", r);
404 kfree(smd_pkt_devp[i]);
405 goto clean_cdevs;
406 }
407
408 smd_pkt_devp[i]->devicep =
409 device_create(smd_pkt_classp, NULL,
410 (smd_pkt_number + i), NULL,
411 smd_pkt_dev_name[i]);
412 if (IS_ERR(smd_pkt_devp[i]->devicep)) {
413 r = PTR_ERR(smd_pkt_devp[i]->devicep);
414 pr_err("device_create() failed %d\n", r);
415 cdev_del(&smd_pkt_devp[i]->cdev);
416 kfree(smd_pkt_devp[i]);
417 goto clean_cdevs;
418 }
419
420 }
421
422 pr_info("SMD Packet Port Driver Initialized.\n");
423 return 0;
424
425clean_cdevs:
426 if (i > 0) {
427 while (--i >= 0) {
428 mutex_destroy(&smd_pkt_devp[i]->ch_lock);
429 mutex_destroy(&smd_pkt_devp[i]->rx_lock);
430 mutex_destroy(&smd_pkt_devp[i]->tx_lock);
431 cdev_del(&smd_pkt_devp[i]->cdev);
432 kfree(smd_pkt_devp[i]);
433 device_destroy(smd_pkt_classp,
434 MKDEV(MAJOR(smd_pkt_number), i));
435 }
436 }
437
438 class_destroy(smd_pkt_classp);
439unreg_chardev:
440 unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
441 return r;
442}
443module_init(smd_pkt_init);
444
445static void __exit smd_pkt_cleanup(void)
446{
447 int i;
448
449 for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
450 mutex_destroy(&smd_pkt_devp[i]->ch_lock);
451 mutex_destroy(&smd_pkt_devp[i]->rx_lock);
452 mutex_destroy(&smd_pkt_devp[i]->tx_lock);
453 cdev_del(&smd_pkt_devp[i]->cdev);
454 kfree(smd_pkt_devp[i]);
455 device_destroy(smd_pkt_classp,
456 MKDEV(MAJOR(smd_pkt_number), i));
457 }
458
459 class_destroy(smd_pkt_classp);
460 unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
461}
462module_exit(smd_pkt_cleanup);
463
464MODULE_DESCRIPTION("MSM Shared Memory Packet Port");
465MODULE_LICENSE("GPL v2");
466
