1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30#include "iforce.h"
31
32void iforce_serial_xmit(struct iforce *iforce)
33{
34 unsigned char cs;
35 int i;
36 unsigned long flags;
37
38 if (test_and_set_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags)) {
39 set_bit(IFORCE_XMIT_AGAIN, iforce->xmit_flags);
40 return;
41 }
42
43 spin_lock_irqsave(&iforce->xmit_lock, flags);
44
45again:
46 if (iforce->xmit.head == iforce->xmit.tail) {
47 clear_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags);
48 spin_unlock_irqrestore(&iforce->xmit_lock, flags);
49 return;
50 }
51
52 cs = 0x2b;
53
54 serio_write(iforce->serio, 0x2b);
55
56 serio_write(iforce->serio, iforce->xmit.buf[iforce->xmit.tail]);
57 cs ^= iforce->xmit.buf[iforce->xmit.tail];
58 XMIT_INC(iforce->xmit.tail, 1);
59
60 for (i=iforce->xmit.buf[iforce->xmit.tail]; i >= 0; --i) {
61 serio_write(iforce->serio, iforce->xmit.buf[iforce->xmit.tail]);
62 cs ^= iforce->xmit.buf[iforce->xmit.tail];
63 XMIT_INC(iforce->xmit.tail, 1);
64 }
65
66 serio_write(iforce->serio, cs);
67
68 if (test_and_clear_bit(IFORCE_XMIT_AGAIN, iforce->xmit_flags))
69 goto again;
70
71 clear_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags);
72
73 spin_unlock_irqrestore(&iforce->xmit_lock, flags);
74}
75
76static void iforce_serio_write_wakeup(struct serio *serio)
77{
78 struct iforce *iforce = serio_get_drvdata(serio);
79
80 iforce_serial_xmit(iforce);
81}
82
83static irqreturn_t iforce_serio_irq(struct serio *serio,
84 unsigned char data, unsigned int flags)
85{
86 struct iforce *iforce = serio_get_drvdata(serio);
87
88 if (!iforce->pkt) {
89 if (data == 0x2b)
90 iforce->pkt = 1;
91 goto out;
92 }
93
94 if (!iforce->id) {
95 if (data > 3 && data != 0xff)
96 iforce->pkt = 0;
97 else
98 iforce->id = data;
99 goto out;
100 }
101
102 if (!iforce->len) {
103 if (data > IFORCE_MAX_LENGTH) {
104 iforce->pkt = 0;
105 iforce->id = 0;
106 } else {
107 iforce->len = data;
108 }
109 goto out;
110 }
111
112 if (iforce->idx < iforce->len) {
113 iforce->csum += iforce->data[iforce->idx++] = data;
114 goto out;
115 }
116
117 if (iforce->idx == iforce->len) {
118 iforce_process_packet(iforce, (iforce->id << 8) | iforce->idx, iforce->data);
119 iforce->pkt = 0;
120 iforce->id = 0;
121 iforce->len = 0;
122 iforce->idx = 0;
123 iforce->csum = 0;
124 }
125out:
126 return IRQ_HANDLED;
127}
128
129static int iforce_serio_connect(struct serio *serio, struct serio_driver *drv)
130{
131 struct iforce *iforce;
132 int err;
133
134 iforce = kzalloc(sizeof(struct iforce), GFP_KERNEL);
135 if (!iforce)
136 return -ENOMEM;
137
138 iforce->bus = IFORCE_232;
139 iforce->serio = serio;
140
141 serio_set_drvdata(serio, iforce);
142
143 err = serio_open(serio, drv);
144 if (err)
145 goto fail1;
146
147 err = iforce_init_device(iforce);
148 if (err)
149 goto fail2;
150
151 return 0;
152
153 fail2: serio_close(serio);
154 fail1: serio_set_drvdata(serio, NULL);
155 kfree(iforce);
156 return err;
157}
158
159static void iforce_serio_disconnect(struct serio *serio)
160{
161 struct iforce *iforce = serio_get_drvdata(serio);
162
163 input_unregister_device(iforce->dev);
164 serio_close(serio);
165 serio_set_drvdata(serio, NULL);
166 kfree(iforce);
167}
168
169static struct serio_device_id iforce_serio_ids[] = {
170 {
171 .type = SERIO_RS232,
172 .proto = SERIO_IFORCE,
173 .id = SERIO_ANY,
174 .extra = SERIO_ANY,
175 },
176 { 0 }
177};
178
179MODULE_DEVICE_TABLE(serio, iforce_serio_ids);
180
181struct serio_driver iforce_serio_drv = {
182 .driver = {
183 .name = "iforce",
184 },
185 .description = "RS232 I-Force joysticks and wheels driver",
186 .id_table = iforce_serio_ids,
187 .write_wakeup = iforce_serio_write_wakeup,
188 .interrupt = iforce_serio_irq,
189 .connect = iforce_serio_connect,
190 .disconnect = iforce_serio_disconnect,
191};
192