2 * Copyright (C) ST-Ericsson AB 2010
3 * Author: Sjur Brendeland
4 * License terms: GNU General Public License (GPL) version 2
7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
9 #include <linux/stddef.h>
10 #include <linux/spinlock.h>
11 #include <linux/slab.h>
12 #include <net/caif/caif_layer.h>
13 #include <net/caif/cfsrvl.h>
14 #include <net/caif/cfpkt.h>
17 #define container_obj(layr) ((struct cfsrvl *) layr)
19 #define DGM_CMD_BIT 0x80
20 #define DGM_FLOW_OFF 0x81
21 #define DGM_FLOW_ON 0x80
24 static int cfdgml_receive(struct cflayer
*layr
, struct cfpkt
*pkt
);
25 static int cfdgml_transmit(struct cflayer
*layr
, struct cfpkt
*pkt
);
27 struct cflayer
*cfdgml_create(u8 channel_id
, struct dev_info
*dev_info
)
29 struct cfsrvl
*dgm
= kzalloc(sizeof(struct cfsrvl
), GFP_ATOMIC
);
32 caif_assert(offsetof(struct cfsrvl
, layer
) == 0);
33 cfsrvl_init(dgm
, channel_id
, dev_info
, true);
34 dgm
->layer
.receive
= cfdgml_receive
;
35 dgm
->layer
.transmit
= cfdgml_transmit
;
36 snprintf(dgm
->layer
.name
, CAIF_LAYER_NAME_SZ
, "dgm%d", channel_id
);
40 static int cfdgml_receive(struct cflayer
*layr
, struct cfpkt
*pkt
)
45 caif_assert(layr
->up
!= NULL
);
46 caif_assert(layr
->receive
!= NULL
);
47 caif_assert(layr
->ctrlcmd
!= NULL
);
49 if (cfpkt_extr_head(pkt
, &cmd
, 1) < 0) {
50 pr_err("Packet is erroneous!\n");
55 if ((cmd
& DGM_CMD_BIT
) == 0) {
56 if (cfpkt_extr_head(pkt
, &dgmhdr
, 3) < 0) {
57 pr_err("Packet is erroneous!\n");
61 ret
= layr
->up
->receive(layr
->up
, pkt
);
66 case DGM_FLOW_OFF
: /* FLOW OFF */
67 layr
->ctrlcmd(layr
, CAIF_CTRLCMD_FLOW_OFF_IND
, 0);
70 case DGM_FLOW_ON
: /* FLOW ON */
71 layr
->ctrlcmd(layr
, CAIF_CTRLCMD_FLOW_ON_IND
, 0);
76 pr_info("Unknown datagram control %d (0x%x)\n", cmd
, cmd
);
81 static int cfdgml_transmit(struct cflayer
*layr
, struct cfpkt
*pkt
)
85 struct caif_payload_info
*info
;
86 struct cfsrvl
*service
= container_obj(layr
);
89 if (!cfsrvl_ready(service
, &ret
)) {
94 /* STE Modem cannot handle more than 1500 bytes datagrams */
95 if (cfpkt_getlen(pkt
) > DGM_MTU
) {
100 cfpkt_add_head(pkt
, &zero
, 3);
101 packet_type
= 0x08; /* B9 set - UNCLASSIFIED */
102 cfpkt_add_head(pkt
, &packet_type
, 1);
104 /* Add info for MUX-layer to route the packet out. */
105 info
= cfpkt_info(pkt
);
106 info
->channel_id
= service
->layer
.id
;
107 /* To optimize alignment, we add up the size of CAIF header
111 info
->dev_info
= &service
->dev_info
;
112 return layr
->dn
->transmit(layr
->dn
, pkt
);