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 | ||
9 | #include <linux/stddef.h> | |
10 | #include <linux/spinlock.h> | |
11 | #include <linux/slab.h> | |
12 | #include <linux/crc-ccitt.h> | |
13 | #include <net/caif/caif_layer.h> | |
14 | #include <net/caif/cfpkt.h> | |
15 | #include <net/caif/cffrml.h> | |
16 | ||
17 | #define container_obj(layr) container_of(layr, struct cffrml, layer) | |
18 | ||
19 | struct cffrml { | |
20 | struct cflayer layer; | |
21 | bool dofcs; /* !< FCS active */ | |
22 | }; | |
23 | ||
24 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt); | |
25 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt); | |
26 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
27 | int phyid); | |
28 | ||
29 | static u32 cffrml_rcv_error; | |
30 | static u32 cffrml_rcv_checsum_error; | |
31 | struct cflayer *cffrml_create(u16 phyid, bool use_fcs) | |
32 | { | |
33 | struct cffrml *this = kmalloc(sizeof(struct cffrml), GFP_ATOMIC); | |
34 | if (!this) { | |
35 | pr_warning("CAIF: %s(): Out of memory\n", __func__); | |
36 | return NULL; | |
37 | } | |
38 | caif_assert(offsetof(struct cffrml, layer) == 0); | |
39 | ||
40 | memset(this, 0, sizeof(struct cflayer)); | |
41 | this->layer.receive = cffrml_receive; | |
42 | this->layer.transmit = cffrml_transmit; | |
43 | this->layer.ctrlcmd = cffrml_ctrlcmd; | |
44 | snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "frm%d", phyid); | |
45 | this->dofcs = use_fcs; | |
46 | this->layer.id = phyid; | |
47 | return (struct cflayer *) this; | |
48 | } | |
49 | ||
50 | void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up) | |
51 | { | |
52 | this->up = up; | |
53 | } | |
54 | ||
55 | void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn) | |
56 | { | |
57 | this->dn = dn; | |
58 | } | |
59 | ||
60 | static u16 cffrml_checksum(u16 chks, void *buf, u16 len) | |
61 | { | |
62 | /* FIXME: FCS should be moved to glue in order to use OS-Specific | |
63 | * solutions | |
64 | */ | |
65 | return crc_ccitt(chks, buf, len); | |
66 | } | |
67 | ||
68 | static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt) | |
69 | { | |
70 | u16 tmp; | |
71 | u16 len; | |
72 | u16 hdrchks; | |
73 | u16 pktchks; | |
74 | struct cffrml *this; | |
75 | this = container_obj(layr); | |
76 | ||
77 | cfpkt_extr_head(pkt, &tmp, 2); | |
78 | len = le16_to_cpu(tmp); | |
79 | ||
80 | /* Subtract for FCS on length if FCS is not used. */ | |
81 | if (!this->dofcs) | |
82 | len -= 2; | |
83 | ||
84 | if (cfpkt_setlen(pkt, len) < 0) { | |
85 | ++cffrml_rcv_error; | |
86 | pr_err("CAIF: %s():Framing length error (%d)\n", __func__, len); | |
87 | cfpkt_destroy(pkt); | |
88 | return -EPROTO; | |
89 | } | |
90 | /* | |
91 | * Don't do extract if FCS is false, rather do setlen - then we don't | |
92 | * get a cache-miss. | |
93 | */ | |
94 | if (this->dofcs) { | |
95 | cfpkt_extr_trail(pkt, &tmp, 2); | |
96 | hdrchks = le16_to_cpu(tmp); | |
97 | pktchks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
98 | if (pktchks != hdrchks) { | |
99 | cfpkt_add_trail(pkt, &tmp, 2); | |
100 | ++cffrml_rcv_error; | |
101 | ++cffrml_rcv_checsum_error; | |
102 | pr_info("CAIF: %s(): Frame checksum error " | |
103 | "(0x%x != 0x%x)\n", __func__, hdrchks, pktchks); | |
104 | return -EILSEQ; | |
105 | } | |
106 | } | |
107 | if (cfpkt_erroneous(pkt)) { | |
108 | ++cffrml_rcv_error; | |
109 | pr_err("CAIF: %s(): Packet is erroneous!\n", __func__); | |
110 | cfpkt_destroy(pkt); | |
111 | return -EPROTO; | |
112 | } | |
113 | return layr->up->receive(layr->up, pkt); | |
114 | } | |
115 | ||
116 | static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt) | |
117 | { | |
118 | int tmp; | |
119 | u16 chks; | |
120 | u16 len; | |
121 | int ret; | |
122 | struct cffrml *this = container_obj(layr); | |
123 | if (this->dofcs) { | |
124 | chks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); | |
125 | tmp = cpu_to_le16(chks); | |
126 | cfpkt_add_trail(pkt, &tmp, 2); | |
127 | } else { | |
128 | cfpkt_pad_trail(pkt, 2); | |
129 | } | |
130 | len = cfpkt_getlen(pkt); | |
131 | tmp = cpu_to_le16(len); | |
132 | cfpkt_add_head(pkt, &tmp, 2); | |
133 | cfpkt_info(pkt)->hdr_len += 2; | |
134 | if (cfpkt_erroneous(pkt)) { | |
135 | pr_err("CAIF: %s(): Packet is erroneous!\n", __func__); | |
136 | return -EPROTO; | |
137 | } | |
138 | ret = layr->dn->transmit(layr->dn, pkt); | |
139 | if (ret < 0) { | |
140 | /* Remove header on faulty packet. */ | |
141 | cfpkt_extr_head(pkt, &tmp, 2); | |
142 | } | |
143 | return ret; | |
144 | } | |
145 | ||
146 | static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
147 | int phyid) | |
148 | { | |
149 | if (layr->up->ctrlcmd) | |
150 | layr->up->ctrlcmd(layr->up, ctrl, layr->id); | |
151 | } |