1 /* 2 * Copyright Gavin Shan, IBM Corporation 2016. 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License as published by 6 * the Free Software Foundation; either version 2 of the License, or 7 * (at your option) any later version. 8 */ 9 10 #include <linux/module.h> 11 #include <linux/kernel.h> 12 #include <linux/init.h> 13 #include <linux/etherdevice.h> 14 #include <linux/netdevice.h> 15 #include <linux/skbuff.h> 16 17 #include <net/ncsi.h> 18 #include <net/net_namespace.h> 19 #include <net/sock.h> 20 #include <net/genetlink.h> 21 22 #include "internal.h" 23 #include "ncsi-pkt.h" 24 25 u32 ncsi_calculate_checksum(unsigned char *data, int len) 26 { 27 u32 checksum = 0; 28 int i; 29 30 for (i = 0; i < len; i += 2) 31 checksum += (((u32)data[i] << 8) | data[i + 1]); 32 33 checksum = (~checksum + 1); 34 return checksum; 35 } 36 37 /* This function should be called after the data area has been 38 * populated completely. 39 */ 40 static void ncsi_cmd_build_header(struct ncsi_pkt_hdr *h, 41 struct ncsi_cmd_arg *nca) 42 { 43 u32 checksum; 44 __be32 *pchecksum; 45 46 h->mc_id = 0; 47 h->revision = NCSI_PKT_REVISION; 48 h->reserved = 0; 49 h->id = nca->id; 50 h->type = nca->type; 51 h->channel = NCSI_TO_CHANNEL(nca->package, 52 nca->channel); 53 h->length = htons(nca->payload); 54 h->reserved1[0] = 0; 55 h->reserved1[1] = 0; 56 57 /* Fill with calculated checksum */ 58 checksum = ncsi_calculate_checksum((unsigned char *)h, 59 sizeof(*h) + nca->payload); 60 pchecksum = (__be32 *)((void *)h + sizeof(struct ncsi_pkt_hdr) + 61 nca->payload); 62 *pchecksum = htonl(checksum); 63 } 64 65 static int ncsi_cmd_handler_default(struct sk_buff *skb, 66 struct ncsi_cmd_arg *nca) 67 { 68 struct ncsi_cmd_pkt *cmd; 69 70 cmd = skb_put_zero(skb, sizeof(*cmd)); 71 ncsi_cmd_build_header(&cmd->cmd.common, nca); 72 73 return 0; 74 } 75 76 static int ncsi_cmd_handler_sp(struct sk_buff *skb, 77 struct ncsi_cmd_arg *nca) 78 { 79 struct ncsi_cmd_sp_pkt *cmd; 80 81 cmd = skb_put_zero(skb, sizeof(*cmd)); 82 cmd->hw_arbitration = nca->bytes[0]; 83 ncsi_cmd_build_header(&cmd->cmd.common, nca); 84 85 return 0; 86 } 87 88 static int ncsi_cmd_handler_dc(struct sk_buff *skb, 89 struct ncsi_cmd_arg *nca) 90 { 91 struct ncsi_cmd_dc_pkt *cmd; 92 93 cmd = skb_put_zero(skb, sizeof(*cmd)); 94 cmd->ald = nca->bytes[0]; 95 ncsi_cmd_build_header(&cmd->cmd.common, nca); 96 97 return 0; 98 } 99 100 static int ncsi_cmd_handler_rc(struct sk_buff *skb, 101 struct ncsi_cmd_arg *nca) 102 { 103 struct ncsi_cmd_rc_pkt *cmd; 104 105 cmd = skb_put_zero(skb, sizeof(*cmd)); 106 ncsi_cmd_build_header(&cmd->cmd.common, nca); 107 108 return 0; 109 } 110 111 static int ncsi_cmd_handler_ae(struct sk_buff *skb, 112 struct ncsi_cmd_arg *nca) 113 { 114 struct ncsi_cmd_ae_pkt *cmd; 115 116 cmd = skb_put_zero(skb, sizeof(*cmd)); 117 cmd->mc_id = nca->bytes[0]; 118 cmd->mode = htonl(nca->dwords[1]); 119 ncsi_cmd_build_header(&cmd->cmd.common, nca); 120 121 return 0; 122 } 123 124 static int ncsi_cmd_handler_sl(struct sk_buff *skb, 125 struct ncsi_cmd_arg *nca) 126 { 127 struct ncsi_cmd_sl_pkt *cmd; 128 129 cmd = skb_put_zero(skb, sizeof(*cmd)); 130 cmd->mode = htonl(nca->dwords[0]); 131 cmd->oem_mode = htonl(nca->dwords[1]); 132 ncsi_cmd_build_header(&cmd->cmd.common, nca); 133 134 return 0; 135 } 136 137 static int ncsi_cmd_handler_svf(struct sk_buff *skb, 138 struct ncsi_cmd_arg *nca) 139 { 140 struct ncsi_cmd_svf_pkt *cmd; 141 142 cmd = skb_put_zero(skb, sizeof(*cmd)); 143 cmd->vlan = htons(nca->words[1]); 144 cmd->index = nca->bytes[6]; 145 cmd->enable = nca->bytes[7]; 146 ncsi_cmd_build_header(&cmd->cmd.common, nca); 147 148 return 0; 149 } 150 151 static int ncsi_cmd_handler_ev(struct sk_buff *skb, 152 struct ncsi_cmd_arg *nca) 153 { 154 struct ncsi_cmd_ev_pkt *cmd; 155 156 cmd = skb_put_zero(skb, sizeof(*cmd)); 157 cmd->mode = nca->bytes[3]; 158 ncsi_cmd_build_header(&cmd->cmd.common, nca); 159 160 return 0; 161 } 162 163 static int ncsi_cmd_handler_sma(struct sk_buff *skb, 164 struct ncsi_cmd_arg *nca) 165 { 166 struct ncsi_cmd_sma_pkt *cmd; 167 int i; 168 169 cmd = skb_put_zero(skb, sizeof(*cmd)); 170 for (i = 0; i < 6; i++) 171 cmd->mac[i] = nca->bytes[i]; 172 cmd->index = nca->bytes[6]; 173 cmd->at_e = nca->bytes[7]; 174 ncsi_cmd_build_header(&cmd->cmd.common, nca); 175 176 return 0; 177 } 178 179 static int ncsi_cmd_handler_ebf(struct sk_buff *skb, 180 struct ncsi_cmd_arg *nca) 181 { 182 struct ncsi_cmd_ebf_pkt *cmd; 183 184 cmd = skb_put_zero(skb, sizeof(*cmd)); 185 cmd->mode = htonl(nca->dwords[0]); 186 ncsi_cmd_build_header(&cmd->cmd.common, nca); 187 188 return 0; 189 } 190 191 static int ncsi_cmd_handler_egmf(struct sk_buff *skb, 192 struct ncsi_cmd_arg *nca) 193 { 194 struct ncsi_cmd_egmf_pkt *cmd; 195 196 cmd = skb_put_zero(skb, sizeof(*cmd)); 197 cmd->mode = htonl(nca->dwords[0]); 198 ncsi_cmd_build_header(&cmd->cmd.common, nca); 199 200 return 0; 201 } 202 203 static int ncsi_cmd_handler_snfc(struct sk_buff *skb, 204 struct ncsi_cmd_arg *nca) 205 { 206 struct ncsi_cmd_snfc_pkt *cmd; 207 208 cmd = skb_put_zero(skb, sizeof(*cmd)); 209 cmd->mode = nca->bytes[0]; 210 ncsi_cmd_build_header(&cmd->cmd.common, nca); 211 212 return 0; 213 } 214 215 static int ncsi_cmd_handler_oem(struct sk_buff *skb, 216 struct ncsi_cmd_arg *nca) 217 { 218 struct ncsi_cmd_oem_pkt *cmd; 219 unsigned int len; 220 221 len = sizeof(struct ncsi_cmd_pkt_hdr) + 4; 222 if (nca->payload < 26) 223 len += 26; 224 else 225 len += nca->payload; 226 227 cmd = skb_put_zero(skb, len); 228 memcpy(&cmd->mfr_id, nca->data, nca->payload); 229 ncsi_cmd_build_header(&cmd->cmd.common, nca); 230 231 return 0; 232 } 233 234 static struct ncsi_cmd_handler { 235 unsigned char type; 236 int payload; 237 int (*handler)(struct sk_buff *skb, 238 struct ncsi_cmd_arg *nca); 239 } ncsi_cmd_handlers[] = { 240 { NCSI_PKT_CMD_CIS, 0, ncsi_cmd_handler_default }, 241 { NCSI_PKT_CMD_SP, 4, ncsi_cmd_handler_sp }, 242 { NCSI_PKT_CMD_DP, 0, ncsi_cmd_handler_default }, 243 { NCSI_PKT_CMD_EC, 0, ncsi_cmd_handler_default }, 244 { NCSI_PKT_CMD_DC, 4, ncsi_cmd_handler_dc }, 245 { NCSI_PKT_CMD_RC, 4, ncsi_cmd_handler_rc }, 246 { NCSI_PKT_CMD_ECNT, 0, ncsi_cmd_handler_default }, 247 { NCSI_PKT_CMD_DCNT, 0, ncsi_cmd_handler_default }, 248 { NCSI_PKT_CMD_AE, 8, ncsi_cmd_handler_ae }, 249 { NCSI_PKT_CMD_SL, 8, ncsi_cmd_handler_sl }, 250 { NCSI_PKT_CMD_GLS, 0, ncsi_cmd_handler_default }, 251 { NCSI_PKT_CMD_SVF, 8, ncsi_cmd_handler_svf }, 252 { NCSI_PKT_CMD_EV, 4, ncsi_cmd_handler_ev }, 253 { NCSI_PKT_CMD_DV, 0, ncsi_cmd_handler_default }, 254 { NCSI_PKT_CMD_SMA, 8, ncsi_cmd_handler_sma }, 255 { NCSI_PKT_CMD_EBF, 4, ncsi_cmd_handler_ebf }, 256 { NCSI_PKT_CMD_DBF, 0, ncsi_cmd_handler_default }, 257 { NCSI_PKT_CMD_EGMF, 4, ncsi_cmd_handler_egmf }, 258 { NCSI_PKT_CMD_DGMF, 0, ncsi_cmd_handler_default }, 259 { NCSI_PKT_CMD_SNFC, 4, ncsi_cmd_handler_snfc }, 260 { NCSI_PKT_CMD_GVI, 0, ncsi_cmd_handler_default }, 261 { NCSI_PKT_CMD_GC, 0, ncsi_cmd_handler_default }, 262 { NCSI_PKT_CMD_GP, 0, ncsi_cmd_handler_default }, 263 { NCSI_PKT_CMD_GCPS, 0, ncsi_cmd_handler_default }, 264 { NCSI_PKT_CMD_GNS, 0, ncsi_cmd_handler_default }, 265 { NCSI_PKT_CMD_GNPTS, 0, ncsi_cmd_handler_default }, 266 { NCSI_PKT_CMD_GPS, 0, ncsi_cmd_handler_default }, 267 { NCSI_PKT_CMD_OEM, -1, ncsi_cmd_handler_oem }, 268 { NCSI_PKT_CMD_PLDM, 0, NULL }, 269 { NCSI_PKT_CMD_GPUUID, 0, ncsi_cmd_handler_default } 270 }; 271 272 static struct ncsi_request *ncsi_alloc_command(struct ncsi_cmd_arg *nca) 273 { 274 struct ncsi_dev_priv *ndp = nca->ndp; 275 struct ncsi_dev *nd = &ndp->ndev; 276 struct net_device *dev = nd->dev; 277 int hlen = LL_RESERVED_SPACE(dev); 278 int tlen = dev->needed_tailroom; 279 int len = hlen + tlen; 280 struct sk_buff *skb; 281 struct ncsi_request *nr; 282 283 nr = ncsi_alloc_request(ndp, nca->req_flags); 284 if (!nr) 285 return NULL; 286 287 /* NCSI command packet has 16-bytes header, payload, 4 bytes checksum. 288 * The packet needs padding if its payload is less than 26 bytes to 289 * meet 64 bytes minimal ethernet frame length. 290 */ 291 len += sizeof(struct ncsi_cmd_pkt_hdr) + 4; 292 if (nca->payload < 26) 293 len += 26; 294 else 295 len += nca->payload; 296 297 /* Allocate skb */ 298 skb = alloc_skb(len, GFP_ATOMIC); 299 if (!skb) { 300 ncsi_free_request(nr); 301 return NULL; 302 } 303 304 nr->cmd = skb; 305 skb_reserve(skb, hlen); 306 skb_reset_network_header(skb); 307 308 skb->dev = dev; 309 skb->protocol = htons(ETH_P_NCSI); 310 311 return nr; 312 } 313 314 int ncsi_xmit_cmd(struct ncsi_cmd_arg *nca) 315 { 316 struct ncsi_request *nr; 317 struct ethhdr *eh; 318 struct ncsi_cmd_handler *nch = NULL; 319 int i, ret; 320 321 /* Search for the handler */ 322 for (i = 0; i < ARRAY_SIZE(ncsi_cmd_handlers); i++) { 323 if (ncsi_cmd_handlers[i].type == nca->type) { 324 if (ncsi_cmd_handlers[i].handler) 325 nch = &ncsi_cmd_handlers[i]; 326 else 327 nch = NULL; 328 329 break; 330 } 331 } 332 333 if (!nch) { 334 netdev_err(nca->ndp->ndev.dev, 335 "Cannot send packet with type 0x%02x\n", nca->type); 336 return -ENOENT; 337 } 338 339 /* Get packet payload length and allocate the request 340 * It is expected that if length set as negative in 341 * handler structure means caller is initializing it 342 * and setting length in nca before calling xmit function 343 */ 344 if (nch->payload >= 0) 345 nca->payload = nch->payload; 346 nr = ncsi_alloc_command(nca); 347 if (!nr) 348 return -ENOMEM; 349 350 /* track netlink information */ 351 if (nca->req_flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) { 352 nr->snd_seq = nca->info->snd_seq; 353 nr->snd_portid = nca->info->snd_portid; 354 nr->nlhdr = *nca->info->nlhdr; 355 } 356 357 /* Prepare the packet */ 358 nca->id = nr->id; 359 ret = nch->handler(nr->cmd, nca); 360 if (ret) { 361 ncsi_free_request(nr); 362 return ret; 363 } 364 365 /* Fill the ethernet header */ 366 eh = skb_push(nr->cmd, sizeof(*eh)); 367 eh->h_proto = htons(ETH_P_NCSI); 368 eth_broadcast_addr(eh->h_dest); 369 eth_broadcast_addr(eh->h_source); 370 371 /* Start the timer for the request that might not have 372 * corresponding response. Given NCSI is an internal 373 * connection a 1 second delay should be sufficient. 374 */ 375 nr->enabled = true; 376 mod_timer(&nr->timer, jiffies + 1 * HZ); 377 378 /* Send NCSI packet */ 379 skb_get(nr->cmd); 380 ret = dev_queue_xmit(nr->cmd); 381 if (ret < 0) { 382 ncsi_free_request(nr); 383 return ret; 384 } 385 386 return 0; 387 } 388