diff options
Diffstat (limited to 'net/caif/cfserl.c')
-rw-r--r-- | net/caif/cfserl.c | 193 |
1 files changed, 193 insertions, 0 deletions
diff --git a/net/caif/cfserl.c b/net/caif/cfserl.c new file mode 100644 index 000000000000..cb4325a3dc83 --- /dev/null +++ b/net/caif/cfserl.c | |||
@@ -0,0 +1,193 @@ | |||
1 | /* | ||
2 | * Copyright (C) ST-Ericsson AB 2010 | ||
3 | * Author: Sjur Brendeland/sjur.brandeland@stericsson.com | ||
4 | * License terms: GNU General Public License (GPL) version 2 | ||
5 | */ | ||
6 | |||
7 | #include <linux/stddef.h> | ||
8 | #include <linux/spinlock.h> | ||
9 | #include <linux/slab.h> | ||
10 | #include <net/caif/caif_layer.h> | ||
11 | #include <net/caif/cfpkt.h> | ||
12 | #include <net/caif/cfserl.h> | ||
13 | |||
14 | #define container_obj(layr) ((struct cfserl *) layr) | ||
15 | |||
16 | #define CFSERL_STX 0x02 | ||
17 | #define CAIF_MINIUM_PACKET_SIZE 4 | ||
18 | struct cfserl { | ||
19 | struct cflayer layer; | ||
20 | struct cfpkt *incomplete_frm; | ||
21 | /* Protects parallel processing of incoming packets */ | ||
22 | spinlock_t sync; | ||
23 | bool usestx; | ||
24 | }; | ||
25 | #define STXLEN(layr) (layr->usestx ? 1 : 0) | ||
26 | |||
27 | static int cfserl_receive(struct cflayer *layr, struct cfpkt *pkt); | ||
28 | static int cfserl_transmit(struct cflayer *layr, struct cfpkt *pkt); | ||
29 | static void cfserl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | ||
30 | int phyid); | ||
31 | |||
32 | struct cflayer *cfserl_create(int type, int instance, bool use_stx) | ||
33 | { | ||
34 | struct cfserl *this = kmalloc(sizeof(struct cfserl), GFP_ATOMIC); | ||
35 | if (!this) { | ||
36 | pr_warning("CAIF: %s(): Out of memory\n", __func__); | ||
37 | return NULL; | ||
38 | } | ||
39 | caif_assert(offsetof(struct cfserl, layer) == 0); | ||
40 | memset(this, 0, sizeof(struct cfserl)); | ||
41 | this->layer.receive = cfserl_receive; | ||
42 | this->layer.transmit = cfserl_transmit; | ||
43 | this->layer.ctrlcmd = cfserl_ctrlcmd; | ||
44 | this->layer.type = type; | ||
45 | this->usestx = use_stx; | ||
46 | spin_lock_init(&this->sync); | ||
47 | snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "ser1"); | ||
48 | return &this->layer; | ||
49 | } | ||
50 | |||
51 | static int cfserl_receive(struct cflayer *l, struct cfpkt *newpkt) | ||
52 | { | ||
53 | struct cfserl *layr = container_obj(l); | ||
54 | u16 pkt_len; | ||
55 | struct cfpkt *pkt = NULL; | ||
56 | struct cfpkt *tail_pkt = NULL; | ||
57 | u8 tmp8; | ||
58 | u16 tmp; | ||
59 | u8 stx = CFSERL_STX; | ||
60 | int ret; | ||
61 | u16 expectlen = 0; | ||
62 | caif_assert(newpkt != NULL); | ||
63 | spin_lock(&layr->sync); | ||
64 | |||
65 | if (layr->incomplete_frm != NULL) { | ||
66 | |||
67 | layr->incomplete_frm = | ||
68 | cfpkt_append(layr->incomplete_frm, newpkt, expectlen); | ||
69 | pkt = layr->incomplete_frm; | ||
70 | if (pkt == NULL) | ||
71 | return -ENOMEM; | ||
72 | } else { | ||
73 | pkt = newpkt; | ||
74 | } | ||
75 | layr->incomplete_frm = NULL; | ||
76 | |||
77 | do { | ||
78 | /* Search for STX at start of pkt if STX is used */ | ||
79 | if (layr->usestx) { | ||
80 | cfpkt_extr_head(pkt, &tmp8, 1); | ||
81 | if (tmp8 != CFSERL_STX) { | ||
82 | while (cfpkt_more(pkt) | ||
83 | && tmp8 != CFSERL_STX) { | ||
84 | cfpkt_extr_head(pkt, &tmp8, 1); | ||
85 | } | ||
86 | if (!cfpkt_more(pkt)) { | ||
87 | cfpkt_destroy(pkt); | ||
88 | layr->incomplete_frm = NULL; | ||
89 | spin_unlock(&layr->sync); | ||
90 | return -EPROTO; | ||
91 | } | ||
92 | } | ||
93 | } | ||
94 | |||
95 | pkt_len = cfpkt_getlen(pkt); | ||
96 | |||
97 | /* | ||
98 | * pkt_len is the accumulated length of the packet data | ||
99 | * we have received so far. | ||
100 | * Exit if frame doesn't hold length. | ||
101 | */ | ||
102 | |||
103 | if (pkt_len < 2) { | ||
104 | if (layr->usestx) | ||
105 | cfpkt_add_head(pkt, &stx, 1); | ||
106 | layr->incomplete_frm = pkt; | ||
107 | spin_unlock(&layr->sync); | ||
108 | return 0; | ||
109 | } | ||
110 | |||
111 | /* | ||
112 | * Find length of frame. | ||
113 | * expectlen is the length we need for a full frame. | ||
114 | */ | ||
115 | cfpkt_peek_head(pkt, &tmp, 2); | ||
116 | expectlen = le16_to_cpu(tmp) + 2; | ||
117 | /* | ||
118 | * Frame error handling | ||
119 | */ | ||
120 | if (expectlen < CAIF_MINIUM_PACKET_SIZE | ||
121 | || expectlen > CAIF_MAX_FRAMESIZE) { | ||
122 | if (!layr->usestx) { | ||
123 | if (pkt != NULL) | ||
124 | cfpkt_destroy(pkt); | ||
125 | layr->incomplete_frm = NULL; | ||
126 | expectlen = 0; | ||
127 | spin_unlock(&layr->sync); | ||
128 | return -EPROTO; | ||
129 | } | ||
130 | continue; | ||
131 | } | ||
132 | |||
133 | if (pkt_len < expectlen) { | ||
134 | /* Too little received data */ | ||
135 | if (layr->usestx) | ||
136 | cfpkt_add_head(pkt, &stx, 1); | ||
137 | layr->incomplete_frm = pkt; | ||
138 | spin_unlock(&layr->sync); | ||
139 | return 0; | ||
140 | } | ||
141 | |||
142 | /* | ||
143 | * Enough data for at least one frame. | ||
144 | * Split the frame, if too long | ||
145 | */ | ||
146 | if (pkt_len > expectlen) | ||
147 | tail_pkt = cfpkt_split(pkt, expectlen); | ||
148 | else | ||
149 | tail_pkt = NULL; | ||
150 | |||
151 | /* Send the first part of packet upwards.*/ | ||
152 | spin_unlock(&layr->sync); | ||
153 | ret = layr->layer.up->receive(layr->layer.up, pkt); | ||
154 | spin_lock(&layr->sync); | ||
155 | if (ret == -EILSEQ) { | ||
156 | if (layr->usestx) { | ||
157 | if (tail_pkt != NULL) | ||
158 | pkt = cfpkt_append(pkt, tail_pkt, 0); | ||
159 | /* Start search for next STX if frame failed */ | ||
160 | continue; | ||
161 | } else { | ||
162 | cfpkt_destroy(pkt); | ||
163 | pkt = NULL; | ||
164 | } | ||
165 | } | ||
166 | |||
167 | pkt = tail_pkt; | ||
168 | |||
169 | } while (pkt != NULL); | ||
170 | |||
171 | spin_unlock(&layr->sync); | ||
172 | return 0; | ||
173 | } | ||
174 | |||
175 | static int cfserl_transmit(struct cflayer *layer, struct cfpkt *newpkt) | ||
176 | { | ||
177 | struct cfserl *layr = container_obj(layer); | ||
178 | int ret; | ||
179 | u8 tmp8 = CFSERL_STX; | ||
180 | if (layr->usestx) | ||
181 | cfpkt_add_head(newpkt, &tmp8, 1); | ||
182 | ret = layer->dn->transmit(layer->dn, newpkt); | ||
183 | if (ret < 0) | ||
184 | cfpkt_extr_head(newpkt, &tmp8, 1); | ||
185 | |||
186 | return ret; | ||
187 | } | ||
188 | |||
189 | static void cfserl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | ||
190 | int phyid) | ||
191 | { | ||
192 | layr->up->ctrlcmd(layr->up, ctrl, phyid); | ||
193 | } | ||