1 // SPDX-License-Identifier: GPL-2.0-only 1 // SPDX-License-Identifier: GPL-2.0-only
2 /* 2 /*
3 * Copyright (C) ST-Ericsson AB 2010 3 * Copyright (C) ST-Ericsson AB 2010
4 * Author: Sjur Brendeland 4 * Author: Sjur Brendeland
5 */ 5 */
6 6
7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " f 7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
8 8
9 #include <linux/stddef.h> 9 #include <linux/stddef.h>
10 #include <linux/spinlock.h> 10 #include <linux/spinlock.h>
11 #include <linux/slab.h> 11 #include <linux/slab.h>
12 #include <net/caif/caif_layer.h> 12 #include <net/caif/caif_layer.h>
13 #include <net/caif/cfsrvl.h> 13 #include <net/caif/cfsrvl.h>
14 #include <net/caif/cfpkt.h> 14 #include <net/caif/cfpkt.h>
15 15
16 16
17 #define container_obj(layr) ((struct cfsrvl *) 17 #define container_obj(layr) ((struct cfsrvl *) layr)
18 18
19 #define DGM_CMD_BIT 0x80 19 #define DGM_CMD_BIT 0x80
20 #define DGM_FLOW_OFF 0x81 20 #define DGM_FLOW_OFF 0x81
21 #define DGM_FLOW_ON 0x80 21 #define DGM_FLOW_ON 0x80
22 #define DGM_MTU 1500 22 #define DGM_MTU 1500
23 23
24 static int cfdgml_receive(struct cflayer *layr 24 static int cfdgml_receive(struct cflayer *layr, struct cfpkt *pkt);
25 static int cfdgml_transmit(struct cflayer *lay 25 static int cfdgml_transmit(struct cflayer *layr, struct cfpkt *pkt);
26 26
27 struct cflayer *cfdgml_create(u8 channel_id, s 27 struct cflayer *cfdgml_create(u8 channel_id, struct dev_info *dev_info)
28 { 28 {
29 struct cfsrvl *dgm = kzalloc(sizeof(st 29 struct cfsrvl *dgm = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
30 if (!dgm) 30 if (!dgm)
31 return NULL; 31 return NULL;
32 caif_assert(offsetof(struct cfsrvl, la 32 caif_assert(offsetof(struct cfsrvl, layer) == 0);
33 cfsrvl_init(dgm, channel_id, dev_info, 33 cfsrvl_init(dgm, channel_id, dev_info, true);
34 dgm->layer.receive = cfdgml_receive; 34 dgm->layer.receive = cfdgml_receive;
35 dgm->layer.transmit = cfdgml_transmit; 35 dgm->layer.transmit = cfdgml_transmit;
36 snprintf(dgm->layer.name, CAIF_LAYER_N 36 snprintf(dgm->layer.name, CAIF_LAYER_NAME_SZ, "dgm%d", channel_id);
37 return &dgm->layer; 37 return &dgm->layer;
38 } 38 }
39 39
40 static int cfdgml_receive(struct cflayer *layr 40 static int cfdgml_receive(struct cflayer *layr, struct cfpkt *pkt)
41 { 41 {
42 u8 cmd = -1; 42 u8 cmd = -1;
43 u8 dgmhdr[3]; 43 u8 dgmhdr[3];
44 int ret; 44 int ret;
45 caif_assert(layr->up != NULL); 45 caif_assert(layr->up != NULL);
46 caif_assert(layr->receive != NULL); 46 caif_assert(layr->receive != NULL);
47 caif_assert(layr->ctrlcmd != NULL); 47 caif_assert(layr->ctrlcmd != NULL);
48 48
49 if (cfpkt_extr_head(pkt, &cmd, 1) < 0) 49 if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
50 pr_err("Packet is erroneous!\n 50 pr_err("Packet is erroneous!\n");
51 cfpkt_destroy(pkt); 51 cfpkt_destroy(pkt);
52 return -EPROTO; 52 return -EPROTO;
53 } 53 }
54 54
55 if ((cmd & DGM_CMD_BIT) == 0) { 55 if ((cmd & DGM_CMD_BIT) == 0) {
56 if (cfpkt_extr_head(pkt, &dgmh 56 if (cfpkt_extr_head(pkt, &dgmhdr, 3) < 0) {
57 pr_err("Packet is erro 57 pr_err("Packet is erroneous!\n");
58 cfpkt_destroy(pkt); 58 cfpkt_destroy(pkt);
59 return -EPROTO; 59 return -EPROTO;
60 } 60 }
61 ret = layr->up->receive(layr-> 61 ret = layr->up->receive(layr->up, pkt);
62 return ret; 62 return ret;
63 } 63 }
64 64
65 switch (cmd) { 65 switch (cmd) {
66 case DGM_FLOW_OFF: /* FLOW OFF */ 66 case DGM_FLOW_OFF: /* FLOW OFF */
67 layr->ctrlcmd(layr, CAIF_CTRLC 67 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
68 cfpkt_destroy(pkt); 68 cfpkt_destroy(pkt);
69 return 0; 69 return 0;
70 case DGM_FLOW_ON: /* FLOW ON */ 70 case DGM_FLOW_ON: /* FLOW ON */
71 layr->ctrlcmd(layr, CAIF_CTRLC 71 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
72 cfpkt_destroy(pkt); 72 cfpkt_destroy(pkt);
73 return 0; 73 return 0;
74 default: 74 default:
75 cfpkt_destroy(pkt); 75 cfpkt_destroy(pkt);
76 pr_info("Unknown datagram cont 76 pr_info("Unknown datagram control %d (0x%x)\n", cmd, cmd);
77 return -EPROTO; 77 return -EPROTO;
78 } 78 }
79 } 79 }
80 80
81 static int cfdgml_transmit(struct cflayer *lay 81 static int cfdgml_transmit(struct cflayer *layr, struct cfpkt *pkt)
82 { 82 {
83 u8 packet_type; 83 u8 packet_type;
84 u32 zero = 0; 84 u32 zero = 0;
85 struct caif_payload_info *info; 85 struct caif_payload_info *info;
86 struct cfsrvl *service = container_obj 86 struct cfsrvl *service = container_obj(layr);
87 int ret; 87 int ret;
88 88
89 if (!cfsrvl_ready(service, &ret)) { 89 if (!cfsrvl_ready(service, &ret)) {
90 cfpkt_destroy(pkt); 90 cfpkt_destroy(pkt);
91 return ret; 91 return ret;
92 } 92 }
93 93
94 /* STE Modem cannot handle more than 1 94 /* STE Modem cannot handle more than 1500 bytes datagrams */
95 if (cfpkt_getlen(pkt) > DGM_MTU) { 95 if (cfpkt_getlen(pkt) > DGM_MTU) {
96 cfpkt_destroy(pkt); 96 cfpkt_destroy(pkt);
97 return -EMSGSIZE; 97 return -EMSGSIZE;
98 } 98 }
99 99
100 cfpkt_add_head(pkt, &zero, 3); 100 cfpkt_add_head(pkt, &zero, 3);
101 packet_type = 0x08; /* B9 set - UNCLAS 101 packet_type = 0x08; /* B9 set - UNCLASSIFIED */
102 cfpkt_add_head(pkt, &packet_type, 1); 102 cfpkt_add_head(pkt, &packet_type, 1);
103 103
104 /* Add info for MUX-layer to route the 104 /* Add info for MUX-layer to route the packet out. */
105 info = cfpkt_info(pkt); 105 info = cfpkt_info(pkt);
106 info->channel_id = service->layer.id; 106 info->channel_id = service->layer.id;
107 /* To optimize alignment, we add up th 107 /* To optimize alignment, we add up the size of CAIF header
108 * before payload. 108 * before payload.
109 */ 109 */
110 info->hdr_len = 4; 110 info->hdr_len = 4;
111 info->dev_info = &service->dev_info; 111 info->dev_info = &service->dev_info;
112 return layr->dn->transmit(layr->dn, pk 112 return layr->dn->transmit(layr->dn, pkt);
113 } 113 }
114 114
Linux® is a registered trademark of Linus Torvalds in the United States and other countries.
TOMOYO® is a registered trademark of NTT DATA CORPORATION.