net/ncsi/ncsi-cmd.c

   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 }