Commit | Line | Data |
---|---|---|
b482cd20 SB |
1 | /* |
2 | * CAIF Framing Layer. | |
3 | * | |
4 | * Copyright (C) ST-Ericsson AB 2010 | |
5 | * Author: Sjur Brendeland/sjur.brandeland@stericsson.com | |
6 | * License terms: GNU General Public License (GPL) version 2 | |
7 | */ | |
8 | ||
b31fa5ba JP |
9 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
10 | ||
b482cd20 SB |
11 | #include <linux/stddef.h> |
12 | #include <linux/spinlock.h> | |
13 | #include <linux/slab.h> | |
14 | #include <linux/crc-ccitt.h> | |
cb3cb423 | 15 | #include <linux/netdevice.h> |
b482cd20 SB |
16 | #include <net/caif/caif_layer.h> |
17 | #include <net/caif/cfpkt.h> | |
18 | #include <net/caif/cffrml.h> | |
19 | ||
20 | #define container_obj(layr) container_of(layr, struct cffrml, layer) | |
21 | ||
22 | struct cffrml { | |
23 | struct cflayer layer; | |
24 | bool dofcs; /* !< FCS active */ | |
cb3cb423 | 25 | int __percpu *pcpu_refcnt; |
b482cd20 SB |
26 | }; |
27 | ||
28 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt); | |
29 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt); | |
30 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
31 | int phyid); | |
32 | ||
33 | static u32 cffrml_rcv_error; | |
34 | static u32 cffrml_rcv_checsum_error; | |
35 | struct cflayer *cffrml_create(u16 phyid, bool use_fcs) | |
cb3cb423 | 36 | |
b482cd20 SB |
37 | { |
38 | struct cffrml *this = kmalloc(sizeof(struct cffrml), GFP_ATOMIC); | |
39 | if (!this) { | |
b31fa5ba | 40 | pr_warn("Out of memory\n"); |
b482cd20 SB |
41 | return NULL; |
42 | } | |
cb3cb423 | 43 | this->pcpu_refcnt = alloc_percpu(int); |
44 | if (this->pcpu_refcnt == NULL) { | |
45 | kfree(this); | |
46 | return NULL; | |
47 | } | |
48 | ||
b482cd20 SB |
49 | caif_assert(offsetof(struct cffrml, layer) == 0); |
50 | ||
51 | memset(this, 0, sizeof(struct cflayer)); | |
52 | this->layer.receive = cffrml_receive; | |
53 | this->layer.transmit = cffrml_transmit; | |
54 | this->layer.ctrlcmd = cffrml_ctrlcmd; | |
55 | snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "frm%d", phyid); | |
56 | this->dofcs = use_fcs; | |
57 | this->layer.id = phyid; | |
58 | return (struct cflayer *) this; | |
59 | } | |
60 | ||
cb3cb423 | 61 | void cffrml_free(struct cflayer *layer) |
62 | { | |
63 | struct cffrml *this = container_obj(layer); | |
64 | free_percpu(this->pcpu_refcnt); | |
65 | kfree(layer); | |
66 | } | |
67 | ||
b482cd20 SB |
68 | void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up) |
69 | { | |
70 | this->up = up; | |
71 | } | |
72 | ||
73 | void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn) | |
74 | { | |
75 | this->dn = dn; | |
76 | } | |
77 | ||
78 | static u16 cffrml_checksum(u16 chks, void *buf, u16 len) | |
79 | { | |
80 | /* FIXME: FCS should be moved to glue in order to use OS-Specific | |
81 | * solutions | |
82 | */ | |
83 | return crc_ccitt(chks, buf, len); | |
84 | } | |
85 | ||
86 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt) | |
87 | { | |
88 | u16 tmp; | |
89 | u16 len; | |
90 | u16 hdrchks; | |
91 | u16 pktchks; | |
92 | struct cffrml *this; | |
93 | this = container_obj(layr); | |
94 | ||
95 | cfpkt_extr_head(pkt, &tmp, 2); | |
96 | len = le16_to_cpu(tmp); | |
97 | ||
98 | /* Subtract for FCS on length if FCS is not used. */ | |
99 | if (!this->dofcs) | |
100 | len -= 2; | |
101 | ||
102 | if (cfpkt_setlen(pkt, len) < 0) { | |
103 | ++cffrml_rcv_error; | |
b31fa5ba | 104 | pr_err("Framing length error (%d)\n", len); |
b482cd20 SB |
105 | cfpkt_destroy(pkt); |
106 | return -EPROTO; | |
107 | } | |
108 | /* | |
109 | * Don't do extract if FCS is false, rather do setlen - then we don't | |
110 | * get a cache-miss. | |
111 | */ | |
112 | if (this->dofcs) { | |
113 | cfpkt_extr_trail(pkt, &tmp, 2); | |
114 | hdrchks = le16_to_cpu(tmp); | |
115 | pktchks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
116 | if (pktchks != hdrchks) { | |
117 | cfpkt_add_trail(pkt, &tmp, 2); | |
118 | ++cffrml_rcv_error; | |
119 | ++cffrml_rcv_checsum_error; | |
b31fa5ba JP |
120 | pr_info("Frame checksum error (0x%x != 0x%x)\n", |
121 | hdrchks, pktchks); | |
b482cd20 SB |
122 | return -EILSEQ; |
123 | } | |
124 | } | |
125 | if (cfpkt_erroneous(pkt)) { | |
126 | ++cffrml_rcv_error; | |
b31fa5ba | 127 | pr_err("Packet is erroneous!\n"); |
b482cd20 SB |
128 | cfpkt_destroy(pkt); |
129 | return -EPROTO; | |
130 | } | |
131 | return layr->up->receive(layr->up, pkt); | |
132 | } | |
133 | ||
134 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt) | |
135 | { | |
136 | int tmp; | |
137 | u16 chks; | |
138 | u16 len; | |
b482cd20 SB |
139 | struct cffrml *this = container_obj(layr); |
140 | if (this->dofcs) { | |
141 | chks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
142 | tmp = cpu_to_le16(chks); | |
143 | cfpkt_add_trail(pkt, &tmp, 2); | |
144 | } else { | |
145 | cfpkt_pad_trail(pkt, 2); | |
146 | } | |
147 | len = cfpkt_getlen(pkt); | |
148 | tmp = cpu_to_le16(len); | |
149 | cfpkt_add_head(pkt, &tmp, 2); | |
150 | cfpkt_info(pkt)->hdr_len += 2; | |
151 | if (cfpkt_erroneous(pkt)) { | |
b31fa5ba | 152 | pr_err("Packet is erroneous!\n"); |
b482cd20 SB |
153 | return -EPROTO; |
154 | } | |
4dd820c0 | 155 | return layr->dn->transmit(layr->dn, pkt); |
b482cd20 SB |
156 | } |
157 | ||
158 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
159 | int phyid) | |
160 | { | |
161 | if (layr->up->ctrlcmd) | |
162 | layr->up->ctrlcmd(layr->up, ctrl, layr->id); | |
163 | } | |
0b1e9738 | 164 | |
165 | void cffrml_put(struct cflayer *layr) | |
166 | { | |
cb3cb423 | 167 | struct cffrml *this = container_obj(layr); |
168 | if (layr != NULL && this->pcpu_refcnt != NULL) | |
169 | irqsafe_cpu_dec(*this->pcpu_refcnt); | |
0b1e9738 | 170 | } |
171 | ||
172 | void cffrml_hold(struct cflayer *layr) | |
173 | { | |
cb3cb423 | 174 | struct cffrml *this = container_obj(layr); |
175 | if (layr != NULL && this->pcpu_refcnt != NULL) | |
176 | irqsafe_cpu_inc(*this->pcpu_refcnt); | |
177 | } | |
178 | ||
179 | int cffrml_refcnt_read(struct cflayer *layr) | |
180 | { | |
181 | int i, refcnt = 0; | |
182 | struct cffrml *this = container_obj(layr); | |
183 | for_each_possible_cpu(i) | |
184 | refcnt += *per_cpu_ptr(this->pcpu_refcnt, i); | |
185 | return refcnt; | |
0b1e9738 | 186 | } |