drivers/net/phy/ncsi.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * NC-SI protocol configuration
   4  *
   5  * Copyright (C) 2019, IBM Corporation.
   6  */
   7
   8 #include <common.h>
   9 #include <log.h>
  10 #include <malloc.h>
  11 #include <phy.h>
  12 #include <net/ncsi.h>
  13 #include <net/ncsi-pkt.h>
  14 #include <asm/unaligned.h>
  15
  16 #define NCSI_PACKAGE_MAX 8
  17 #define NCSI_CHANNEL_MAX 31
  18
  19 #define NCSI_PACKAGE_SHIFT      5
  20 #define NCSI_PACKAGE_INDEX(c)   (((c) >> NCSI_PACKAGE_SHIFT) & 0x7)
  21 #define NCSI_RESERVED_CHANNEL   0x1f
  22 #define NCSI_CHANNEL_INDEX(c)   ((c) & ((1 << NCSI_PACKAGE_SHIFT) - 1))
  23 #define NCSI_TO_CHANNEL(p, c)   (((p) << NCSI_PACKAGE_SHIFT) | (c))
  24
  25 #define NCSI_PKT_REVISION       0x01
  26
  27 #define NCSI_CAP_GENERIC_MASK   0x7f
  28 #define NCSI_CAP_BC_MASK        0x0f
  29 #define NCSI_CAP_MC_MASK        0x3f
  30 #define NCSI_CAP_AEN_MASK       0x07
  31 #define NCSI_CAP_VLAN_MASK      0x07
  32
  33 static void ncsi_send_ebf(unsigned int np, unsigned int nc);
  34 static void ncsi_send_ae(unsigned int np, unsigned int nc);
  35 static void ncsi_send_gls(unsigned int np, unsigned int nc);
  36 static int ncsi_send_command(unsigned int np, unsigned int nc, unsigned int cmd,
  37                              uchar *payload, int len, bool wait);
  38
  39 struct ncsi_channel {
  40         unsigned int    id;
  41         bool            has_link;
  42
  43         /* capabilities */
  44         u32 cap_generic;
  45         u32 cap_bc;
  46         u32 cap_mc;
  47         u32 cap_buffer;
  48         u32 cap_aen;
  49         u32 cap_vlan;
  50
  51         /* version information */
  52         struct {
  53                 u32 version;            /* Supported BCD encoded NCSI version */
  54                 u32 alpha2;             /* Supported BCD encoded NCSI version */
  55                 u8  fw_name[12];        /* Firmware name string               */
  56                 u32 fw_version;         /* Firmware version                   */
  57                 u16 pci_ids[4];         /* PCI identification                 */
  58                 u32 mf_id;              /* Manufacture ID                     */
  59         } version;
  60
  61 };
  62
  63 struct ncsi_package {
  64         unsigned int            id;
  65         unsigned int            n_channels;
  66         struct ncsi_channel     *channels;
  67 };
  68
  69 struct ncsi {
  70         enum {
  71                 NCSI_PROBE_PACKAGE_SP,
  72                 NCSI_PROBE_PACKAGE_DP,
  73                 NCSI_PROBE_CHANNEL_SP,
  74                 NCSI_PROBE_CHANNEL,
  75                 NCSI_CONFIG,
  76         } state;
  77
  78         unsigned int    pending_requests;
  79         unsigned int    requests[256];
  80         unsigned int    last_request;
  81
  82         unsigned int    current_package;
  83         unsigned int    current_channel;
  84
  85         unsigned int            n_packages;
  86         struct ncsi_package     *packages;
  87 };
  88
  89 struct ncsi *ncsi_priv;
  90
  91 bool ncsi_active(void)
  92 {
  93         unsigned int np, nc;
  94
  95         if (!ncsi_priv)
  96                 return false;
  97
  98         np = ncsi_priv->current_package;
  99         nc = ncsi_priv->current_channel;
 100
 101         if (ncsi_priv->state != NCSI_CONFIG)
 102                 return false;
 103
 104         return np < NCSI_PACKAGE_MAX && nc < NCSI_CHANNEL_MAX &&
 105                 ncsi_priv->packages[np].channels[nc].has_link;
 106 }
 107
 108 static unsigned int cmd_payload(int cmd)
 109 {
 110         switch (cmd) {
 111         case NCSI_PKT_CMD_CIS:
 112                 return 0;
 113         case NCSI_PKT_CMD_SP:
 114                 return 4;
 115         case NCSI_PKT_CMD_DP:
 116                 return 0;
 117         case NCSI_PKT_CMD_EC:
 118                 return 0;
 119         case NCSI_PKT_CMD_DC:
 120                 return 4;
 121         case NCSI_PKT_CMD_RC:
 122                 return 4;
 123         case NCSI_PKT_CMD_ECNT:
 124                 return 0;
 125         case NCSI_PKT_CMD_DCNT:
 126                 return 0;
 127         case NCSI_PKT_CMD_AE:
 128                 return 8;
 129         case NCSI_PKT_CMD_SL:
 130                 return 8;
 131         case NCSI_PKT_CMD_GLS:
 132                 return 0;
 133         case NCSI_PKT_CMD_SVF:
 134                 return 8;
 135         case NCSI_PKT_CMD_EV:
 136                 return 4;
 137         case NCSI_PKT_CMD_DV:
 138                 return 0;
 139         case NCSI_PKT_CMD_SMA:
 140                 return 8;
 141         case NCSI_PKT_CMD_EBF:
 142                 return 4;
 143         case NCSI_PKT_CMD_DBF:
 144                 return 0;
 145         case NCSI_PKT_CMD_EGMF:
 146                 return 4;
 147         case NCSI_PKT_CMD_DGMF:
 148                 return 0;
 149         case NCSI_PKT_CMD_SNFC:
 150                 return 4;
 151         case NCSI_PKT_CMD_GVI:
 152                 return 0;
 153         case NCSI_PKT_CMD_GC:
 154                 return 0;
 155         case NCSI_PKT_CMD_GP:
 156                 return 0;
 157         case NCSI_PKT_CMD_GCPS:
 158                 return 0;
 159         case NCSI_PKT_CMD_GNS:
 160                 return 0;
 161         case NCSI_PKT_CMD_GNPTS:
 162                 return 0;
 163         case NCSI_PKT_CMD_GPS:
 164                 return 0;
 165         default:
 166                 printf("NCSI: Unknown command 0x%02x\n", cmd);
 167                 return 0;
 168         }
 169 }
 170
 171 static u32 ncsi_calculate_checksum(unsigned char *data, int len)
 172 {
 173         u32 checksum = 0;
 174         int i;
 175
 176         for (i = 0; i < len; i += 2)
 177                 checksum += (((u32)data[i] << 8) | data[i + 1]);
 178
 179         checksum = (~checksum + 1);
 180         return checksum;
 181 }
 182
 183 static int ncsi_validate_rsp(struct ncsi_rsp_pkt *pkt, int payload)
 184 {
 185         struct ncsi_rsp_pkt_hdr *hdr = &pkt->rsp;
 186         u32 checksum, c_offset;
 187         __be32 pchecksum;
 188
 189         if (hdr->common.revision != 1) {
 190                 printf("NCSI: 0x%02x response has unsupported revision 0x%x\n",
 191                        hdr->common.type, hdr->common.revision);
 192                 return -1;
 193         }
 194
 195         if (hdr->code != 0) {
 196                 printf("NCSI: 0x%02x response returns error %d\n",
 197                        hdr->common.type, __be16_to_cpu(hdr->code));
 198                 if (ntohs(hdr->reason) == 0x05)
 199                         printf("(Invalid command length)\n");
 200                 return -1;
 201         }
 202
 203         if (ntohs(hdr->common.length) != payload) {
 204                 printf("NCSI: 0x%02x response has incorrect length %d\n",
 205                        hdr->common.type, hdr->common.length);
 206                 return -1;
 207         }
 208
 209         c_offset = sizeof(struct ncsi_rsp_pkt_hdr) + payload - sizeof(checksum);
 210         pchecksum = get_unaligned_be32((void *)hdr + c_offset);
 211         if (pchecksum != 0) {
 212                 checksum = ncsi_calculate_checksum((unsigned char *)hdr,
 213                                                    c_offset);
 214                 if (pchecksum != checksum) {
 215                         printf("NCSI: 0x%02x response has invalid checksum\n",
 216                                hdr->common.type);
 217                         return -1;
 218                 }
 219         }
 220
 221         return 0;
 222 }
 223
 224 static void ncsi_rsp_ec(struct ncsi_rsp_pkt *pkt)
 225 {
 226         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)&pkt->rsp;
 227         unsigned int np, nc;
 228
 229         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 230         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 231
 232         if (ncsi_priv->packages[np].channels[nc].cap_aen != 0)
 233                 ncsi_send_ae(np, nc);
 234         /* else, done */
 235 }
 236
 237 static void ncsi_rsp_ecnt(struct ncsi_rsp_pkt *pkt)
 238 {
 239         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)&pkt->rsp;
 240         unsigned int np, nc;
 241
 242         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 243         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 244
 245         ncsi_send_command(np, nc, NCSI_PKT_CMD_EC, NULL, 0, true);
 246 }
 247
 248 static void ncsi_rsp_ebf(struct ncsi_rsp_pkt *pkt)
 249 {
 250         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)&pkt->rsp;
 251         unsigned int np, nc;
 252
 253         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 254         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 255
 256         ncsi_send_command(np, nc, NCSI_PKT_CMD_ECNT, NULL, 0, true);
 257 }
 258
 259 static void ncsi_rsp_sma(struct ncsi_rsp_pkt *pkt)
 260 {
 261         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)&pkt->rsp;
 262         unsigned int np, nc;
 263
 264         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 265         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 266
 267         ncsi_send_ebf(np, nc);
 268 }
 269
 270 static void ncsi_rsp_gc(struct ncsi_rsp_pkt *pkt)
 271 {
 272         struct ncsi_rsp_gc_pkt *gc = (struct ncsi_rsp_gc_pkt *)pkt;
 273         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)&gc->rsp;
 274         struct ncsi_channel *c;
 275         unsigned int np, nc;
 276
 277         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 278         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 279
 280         if (np >= ncsi_priv->n_packages ||
 281             nc >= ncsi_priv->packages[np].n_channels) {
 282                 printf("NCSI: Invalid package / channel (0x%02x, 0x%02x)\n",
 283                        np, nc);
 284                 return;
 285         }
 286
 287         c = &ncsi_priv->packages[np].channels[nc];
 288         c->cap_generic = ntohl(gc->cap) & NCSI_CAP_GENERIC_MASK;
 289         c->cap_bc = ntohl(gc->bc_cap) & NCSI_CAP_BC_MASK;
 290         c->cap_mc = ntohl(gc->mc_cap) & NCSI_CAP_MC_MASK;
 291         c->cap_aen = ntohl(gc->aen_cap) & NCSI_CAP_AEN_MASK;
 292         c->cap_vlan = ntohl(gc->vlan_mode) & NCSI_CAP_VLAN_MASK;
 293
 294         /* End of probe for this channel */
 295 }
 296
 297 static void ncsi_rsp_gvi(struct ncsi_rsp_pkt *pkt)
 298 {
 299         struct ncsi_rsp_gvi_pkt *gvi = (struct ncsi_rsp_gvi_pkt *)pkt;
 300         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)&gvi->rsp;
 301         struct ncsi_channel *c;
 302         unsigned int np, nc, i;
 303
 304         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 305         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 306
 307         if (np >= ncsi_priv->n_packages ||
 308             nc >= ncsi_priv->packages[np].n_channels) {
 309                 printf("NCSI: Invalid package / channel (0x%02x, 0x%02x)\n",
 310                        np, nc);
 311                 return;
 312         }
 313
 314         c = &ncsi_priv->packages[np].channels[nc];
 315         c->version.version = get_unaligned_be32(&gvi->ncsi_version);
 316         c->version.alpha2 = gvi->alpha2;
 317         memcpy(c->version.fw_name, gvi->fw_name, sizeof(c->version.fw_name));
 318         c->version.fw_version = get_unaligned_be32(&gvi->fw_version);
 319         for (i = 0; i < ARRAY_SIZE(c->version.pci_ids); i++)
 320                 c->version.pci_ids[i] = get_unaligned_be16(gvi->pci_ids + i);
 321         c->version.mf_id = get_unaligned_be32(&gvi->mf_id);
 322
 323         if (ncsi_priv->state == NCSI_PROBE_CHANNEL)
 324                 ncsi_send_command(np, nc, NCSI_PKT_CMD_GC, NULL, 0, true);
 325 }
 326
 327 static void ncsi_rsp_gls(struct ncsi_rsp_pkt *pkt)
 328 {
 329         struct ncsi_rsp_gls_pkt *gls = (struct ncsi_rsp_gls_pkt *)pkt;
 330         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)&gls->rsp;
 331         unsigned int np, nc;
 332
 333         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 334         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 335
 336         if (np >= ncsi_priv->n_packages ||
 337             nc >= ncsi_priv->packages[np].n_channels) {
 338                 printf("NCSI: Invalid package / channel (0x%02x, 0x%02x)\n",
 339                        np, nc);
 340                 return;
 341         }
 342
 343         ncsi_priv->packages[np].channels[nc].has_link =
 344                                         !!(get_unaligned_be32(&gls->status));
 345
 346         if (ncsi_priv->state == NCSI_PROBE_CHANNEL)
 347                 ncsi_send_command(np, nc, NCSI_PKT_CMD_GVI, NULL, 0, true);
 348 }
 349
 350 static void ncsi_rsp_cis(struct ncsi_rsp_pkt *pkt)
 351 {
 352         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)pkt;
 353         struct ncsi_package *package;
 354         unsigned int np, nc;
 355
 356         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 357         nc = NCSI_CHANNEL_INDEX(rsp->common.channel);
 358
 359         if (np >= ncsi_priv->n_packages) {
 360                 printf("NCSI: Mystery package 0x%02x from CIS\n", np);
 361                 return;
 362         }
 363
 364         package = &ncsi_priv->packages[np];
 365
 366         if (nc < package->n_channels) {
 367                 /*
 368                  * This is fine in general but in the current design we
 369                  * don't send CIS commands to known channels.
 370                  */
 371                 debug("NCSI: Duplicate channel 0x%02x\n", nc);
 372                 return;
 373         }
 374
 375         package->channels = realloc(package->channels,
 376                                     sizeof(struct ncsi_channel) *
 377                                     (package->n_channels + 1));
 378         if (!package->channels) {
 379                 printf("NCSI: Could not allocate memory for new channel\n");
 380                 return;
 381         }
 382
 383         debug("NCSI: New channel 0x%02x\n", nc);
 384
 385         package->channels[nc].id = nc;
 386         package->channels[nc].has_link = false;
 387         package->n_channels++;
 388
 389         ncsi_send_gls(np, nc);
 390 }
 391
 392 static void ncsi_rsp_dp(struct ncsi_rsp_pkt *pkt)
 393 {
 394         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)pkt;
 395         unsigned int np;
 396
 397         /* No action needed */
 398
 399         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 400         if (np >= ncsi_priv->n_packages)
 401                 debug("NCSI: DP response from unknown package %d\n", np);
 402 }
 403
 404 static void ncsi_rsp_sp(struct ncsi_rsp_pkt *pkt)
 405 {
 406         struct ncsi_rsp_pkt_hdr *rsp = (struct ncsi_rsp_pkt_hdr *)pkt;
 407         unsigned int np;
 408
 409         np = NCSI_PACKAGE_INDEX(rsp->common.channel);
 410
 411         if (np < ncsi_priv->n_packages) {
 412                 /* Already know about this package */
 413                 debug("NCSI: package 0x%02x selected\n", np);
 414                 return;
 415         }
 416
 417         debug("NCSI: adding new package %d\n", np);
 418
 419         ncsi_priv->packages = realloc(ncsi_priv->packages,
 420                                       sizeof(struct ncsi_package) *
 421                                       (ncsi_priv->n_packages + 1));
 422         if (!ncsi_priv->packages) {
 423                 printf("NCSI: could not allocate memory for new package\n");
 424                 return;
 425         }
 426
 427         ncsi_priv->packages[np].id = np;
 428         ncsi_priv->packages[np].n_channels = 0;
 429         ncsi_priv->packages[np].channels = NULL;
 430         ncsi_priv->n_packages++;
 431 }
 432
 433 static void ncsi_update_state(struct ncsi_rsp_pkt_hdr *nh)
 434 {
 435         bool timeout = !nh;
 436         int np, nc;
 437
 438         switch (ncsi_priv->state) {
 439         case NCSI_PROBE_PACKAGE_SP:
 440                 if (!timeout &&
 441                     ncsi_priv->current_package + 1 < NCSI_PACKAGE_MAX) {
 442                         ncsi_priv->current_package++;
 443                 } else {
 444                         ncsi_priv->state = NCSI_PROBE_PACKAGE_DP;
 445                         ncsi_priv->current_package = 0;
 446                 }
 447                 return ncsi_probe_packages();
 448         case NCSI_PROBE_PACKAGE_DP:
 449                 if (ncsi_priv->current_package + 1 < ncsi_priv->n_packages &&
 450                     !timeout) {
 451                         ncsi_priv->current_package++;
 452                 } else {
 453                         if (!ncsi_priv->n_packages) {
 454                                 printf("NCSI: no packages found\n");
 455                                 net_set_state(NETLOOP_FAIL);
 456                                 return;
 457                         }
 458                         printf("NCSI: probing channels\n");
 459                         ncsi_priv->state = NCSI_PROBE_CHANNEL_SP;
 460                         ncsi_priv->current_package = 0;
 461                         ncsi_priv->current_channel = 0;
 462                 }
 463                 return ncsi_probe_packages();
 464         case NCSI_PROBE_CHANNEL_SP:
 465                 if (!timeout && nh->common.type == NCSI_PKT_RSP_SP) {
 466                         ncsi_priv->state = NCSI_PROBE_CHANNEL;
 467                         return ncsi_probe_packages();
 468                 }
 469                 printf("NCSI: failed to select package 0x%0x2 or timeout\n",
 470                        ncsi_priv->current_package);
 471                 net_set_state(NETLOOP_FAIL);
 472                 break;
 473         case NCSI_PROBE_CHANNEL:
 474                 // TODO only does package 0 for now
 475                 if (ncsi_priv->pending_requests == 0) {
 476                         np = ncsi_priv->current_package;
 477                         nc = ncsi_priv->current_channel;
 478
 479                         /* Configure first channel that has link */
 480                         if (ncsi_priv->packages[np].channels[nc].has_link) {
 481                                 ncsi_priv->state = NCSI_CONFIG;
 482                         } else if (ncsi_priv->current_channel + 1 <
 483                                    NCSI_CHANNEL_MAX) {
 484                                 ncsi_priv->current_channel++;
 485                         } else {
 486                                 // XXX As above only package 0
 487                                 printf("NCSI: no channel found with link\n");
 488                                 net_set_state(NETLOOP_FAIL);
 489                                 return;
 490                         }
 491                         return ncsi_probe_packages();
 492                 }
 493                 break;
 494         case NCSI_CONFIG:
 495                 if (ncsi_priv->pending_requests == 0) {
 496                         printf("NCSI: configuration done!\n");
 497                         net_set_state(NETLOOP_SUCCESS);
 498                 } else if (timeout) {
 499                         printf("NCSI: timeout during configure\n");
 500                         net_set_state(NETLOOP_FAIL);
 501                 }
 502                 break;
 503         default:
 504                 printf("NCSI: something went very wrong, nevermind\n");
 505                 net_set_state(NETLOOP_FAIL);
 506                 break;
 507         }
 508 }
 509
 510 static void ncsi_timeout_handler(void)
 511 {
 512         if (ncsi_priv->pending_requests)
 513                 ncsi_priv->pending_requests--;
 514
 515         ncsi_update_state(NULL);
 516 }
 517
 518 static int ncsi_send_command(unsigned int np, unsigned int nc, unsigned int cmd,
 519                              uchar *payload, int len, bool wait)
 520 {
 521         struct ncsi_pkt_hdr *hdr;
 522         __be32 *pchecksum;
 523         int eth_hdr_size;
 524         u32 checksum;
 525         uchar *pkt, *start;
 526         int final_len;
 527
 528         pkt = calloc(1, PKTSIZE_ALIGN + PKTALIGN);
 529         if (!pkt)
 530                 return -ENOMEM;
 531         start = pkt;
 532
 533         eth_hdr_size = net_set_ether(pkt, net_bcast_ethaddr, PROT_NCSI);
 534         pkt += eth_hdr_size;
 535
 536         /* Set NCSI command header fields */
 537         hdr = (struct ncsi_pkt_hdr *)pkt;
 538         hdr->mc_id = 0;
 539         hdr->revision = NCSI_PKT_REVISION;
 540         hdr->id = ++ncsi_priv->last_request;
 541         ncsi_priv->requests[ncsi_priv->last_request] = 1;
 542         hdr->type = cmd;
 543         hdr->channel = NCSI_TO_CHANNEL(np, nc);
 544         hdr->length = htons(len);
 545
 546         if (payload && len)
 547                 memcpy(pkt + sizeof(struct ncsi_pkt_hdr), payload, len);
 548
 549         /* Calculate checksum */
 550         checksum = ncsi_calculate_checksum((unsigned char *)hdr,
 551                                            sizeof(*hdr) + len);
 552         pchecksum = (__be32 *)((void *)(hdr + 1) + len);
 553         put_unaligned_be32(htonl(checksum), pchecksum);
 554
 555         if (wait) {
 556                 net_set_timeout_handler(1000UL, ncsi_timeout_handler);
 557                 ncsi_priv->pending_requests++;
 558         }
 559
 560         if (len < 26)
 561                 len = 26;
 562         /* frame header, packet header, payload, checksum */
 563         final_len = eth_hdr_size + sizeof(struct ncsi_cmd_pkt_hdr) + len + 4;
 564
 565         net_send_packet(start, final_len);
 566         free(start);
 567         return 0;
 568 }
 569
 570 static void ncsi_handle_aen(struct ip_udp_hdr *ip, unsigned int len)
 571 {
 572         struct ncsi_aen_pkt_hdr *hdr = (struct ncsi_aen_pkt_hdr *)ip;
 573         int payload, i;
 574         __be32 pchecksum;
 575         u32 checksum;
 576
 577         switch (hdr->type) {
 578         case NCSI_PKT_AEN_LSC:
 579                 printf("NCSI: link state changed\n");
 580                 payload = 12;
 581                 break;
 582         case NCSI_PKT_AEN_CR:
 583                 printf("NCSI: re-configuration required\n");
 584                 payload = 4;
 585                 break;
 586         case NCSI_PKT_AEN_HNCDSC:
 587                 /* Host notifcation - N/A but weird */
 588                 debug("NCSI: HNCDSC AEN received\n");
 589                 return;
 590         default:
 591                 printf("%s: Invalid type 0x%02x\n", __func__, hdr->type);
 592                 return;
 593         }
 594
 595         /* Validate packet */
 596         if (hdr->common.revision != 1) {
 597                 printf("NCSI: 0x%02x response has unsupported revision 0x%x\n",
 598                        hdr->common.type, hdr->common.revision);
 599                 return;
 600         }
 601
 602         if (ntohs(hdr->common.length) != payload) {
 603                 printf("NCSI: 0x%02x response has incorrect length %d\n",
 604                        hdr->common.type, hdr->common.length);
 605                 return;
 606         }
 607
 608         pchecksum = get_unaligned_be32((void *)(hdr + 1) + payload - 4);
 609         if (pchecksum != 0) {
 610                 checksum = ncsi_calculate_checksum((unsigned char *)hdr,
 611                                                    sizeof(*hdr) + payload - 4);
 612                 if (pchecksum != checksum) {
 613                         printf("NCSI: 0x%02x response has invalid checksum\n",
 614                                hdr->common.type);
 615                         return;
 616                 }
 617         }
 618
 619         /* Link or configuration lost - just redo the discovery process */
 620         ncsi_priv->state = NCSI_PROBE_PACKAGE_SP;
 621         for (i = 0; i < ncsi_priv->n_packages; i++)
 622                 free(ncsi_priv->packages[i].channels);
 623         free(ncsi_priv->packages);
 624         ncsi_priv->n_packages = 0;
 625
 626         ncsi_priv->current_package = NCSI_PACKAGE_MAX;
 627         ncsi_priv->current_channel = NCSI_CHANNEL_MAX;
 628
 629         ncsi_probe_packages();
 630 }
 631
 632 void ncsi_receive(struct ethernet_hdr *et, struct ip_udp_hdr *ip,
 633                   unsigned int len)
 634 {
 635         struct ncsi_rsp_pkt *pkt = (struct ncsi_rsp_pkt *)ip;
 636         struct ncsi_rsp_pkt_hdr *nh = (struct ncsi_rsp_pkt_hdr *)&pkt->rsp;
 637         void (*handler)(struct ncsi_rsp_pkt *pkt) = NULL;
 638         unsigned short payload;
 639
 640         if (ncsi_priv->pending_requests)
 641                 ncsi_priv->pending_requests--;
 642
 643         if (len < sizeof(struct ncsi_rsp_pkt_hdr)) {
 644                 printf("NCSI: undersized packet: %u bytes\n", len);
 645                 goto out;
 646         }
 647
 648         if (nh->common.type == NCSI_PKT_AEN)
 649                 return ncsi_handle_aen(ip, len);
 650
 651         switch (nh->common.type) {
 652         case NCSI_PKT_RSP_SP:
 653                 payload = 4;
 654                 handler = ncsi_rsp_sp;
 655                 break;
 656         case NCSI_PKT_RSP_DP:
 657                 payload = 4;
 658                 handler = ncsi_rsp_dp;
 659                 break;
 660         case NCSI_PKT_RSP_CIS:
 661                 payload = 4;
 662                 handler = ncsi_rsp_cis;
 663                 break;
 664         case NCSI_PKT_RSP_GLS:
 665                 payload = 16;
 666                 handler = ncsi_rsp_gls;
 667                 break;
 668         case NCSI_PKT_RSP_GVI:
 669                 payload = 40;
 670                 handler = ncsi_rsp_gvi;
 671                 break;
 672         case NCSI_PKT_RSP_GC:
 673                 payload = 32;
 674                 handler = ncsi_rsp_gc;
 675                 break;
 676         case NCSI_PKT_RSP_SMA:
 677                 payload = 4;
 678                 handler = ncsi_rsp_sma;
 679                 break;
 680         case NCSI_PKT_RSP_EBF:
 681                 payload = 4;
 682                 handler = ncsi_rsp_ebf;
 683                 break;
 684         case NCSI_PKT_RSP_ECNT:
 685                 payload = 4;
 686                 handler = ncsi_rsp_ecnt;
 687                 break;
 688         case NCSI_PKT_RSP_EC:
 689                 payload = 4;
 690                 handler = ncsi_rsp_ec;
 691                 break;
 692         case NCSI_PKT_RSP_AE:
 693                 payload = 4;
 694                 handler = NULL;
 695                 break;
 696         default:
 697                 printf("NCSI: unsupported packet type 0x%02x\n",
 698                        nh->common.type);
 699                 goto out;
 700         }
 701
 702         if (ncsi_validate_rsp(pkt, payload) != 0) {
 703                 printf("NCSI: discarding invalid packet of type 0x%02x\n",
 704                        nh->common.type);
 705                 goto out;
 706         }
 707
 708         if (handler)
 709                 handler(pkt);
 710 out:
 711         ncsi_update_state(nh);
 712 }
 713
 714 static void ncsi_send_sp(unsigned int np)
 715 {
 716         uchar payload[4] = {0};
 717
 718         ncsi_send_command(np, NCSI_RESERVED_CHANNEL, NCSI_PKT_CMD_SP,
 719                           (unsigned char *)&payload,
 720                           cmd_payload(NCSI_PKT_CMD_SP), true);
 721 }
 722
 723 static void ncsi_send_dp(unsigned int np)
 724 {
 725         ncsi_send_command(np, NCSI_RESERVED_CHANNEL, NCSI_PKT_CMD_DP, NULL, 0,
 726                           true);
 727 }
 728
 729 static void ncsi_send_gls(unsigned int np, unsigned int nc)
 730 {
 731         ncsi_send_command(np, nc, NCSI_PKT_CMD_GLS, NULL, 0, true);
 732 }
 733
 734 static void ncsi_send_cis(unsigned int np, unsigned int nc)
 735 {
 736         ncsi_send_command(np, nc, NCSI_PKT_CMD_CIS, NULL, 0, true);
 737 }
 738
 739 static void ncsi_send_ae(unsigned int np, unsigned int nc)
 740 {
 741         struct ncsi_cmd_ae_pkt cmd;
 742
 743         memset(&cmd, 0, sizeof(cmd));
 744         cmd.mode = htonl(ncsi_priv->packages[np].channels[nc].cap_aen);
 745
 746         ncsi_send_command(np, nc, NCSI_PKT_CMD_AE,
 747                           ((unsigned char *)&cmd)
 748                           + sizeof(struct ncsi_cmd_pkt_hdr),
 749                           cmd_payload(NCSI_PKT_CMD_AE), true);
 750 }
 751
 752 static void ncsi_send_ebf(unsigned int np, unsigned int nc)
 753 {
 754         struct ncsi_cmd_ebf_pkt cmd;
 755
 756         memset(&cmd, 0, sizeof(cmd));
 757         cmd.mode = htonl(ncsi_priv->packages[np].channels[nc].cap_bc);
 758
 759         ncsi_send_command(np, nc, NCSI_PKT_CMD_EBF,
 760                           ((unsigned char *)&cmd)
 761                           + sizeof(struct ncsi_cmd_pkt_hdr),
 762                           cmd_payload(NCSI_PKT_CMD_EBF), true);
 763 }
 764
 765 static void ncsi_send_sma(unsigned int np, unsigned int nc)
 766 {
 767         struct ncsi_cmd_sma_pkt cmd;
 768         unsigned char *addr, i;
 769
 770         addr = eth_get_ethaddr();
 771         if (!addr) {
 772                 printf("NCSI: no MAC address configured\n");
 773                 return;
 774         }
 775
 776         memset(&cmd, 0, sizeof(cmd));
 777         for (i = 0; i < ARP_HLEN; i++)
 778                 cmd.mac[i] = addr[i];
 779         cmd.index = 1;
 780         cmd.at_e = 1;
 781
 782         ncsi_send_command(np, nc, NCSI_PKT_CMD_SMA,
 783                           ((unsigned char *)&cmd)
 784                           + sizeof(struct ncsi_cmd_pkt_hdr),
 785                           cmd_payload(NCSI_PKT_CMD_SMA), true);
 786 }
 787
 788 void ncsi_probe_packages(void)
 789 {
 790         struct ncsi_package *package;
 791         unsigned int np, nc;
 792
 793         switch (ncsi_priv->state) {
 794         case NCSI_PROBE_PACKAGE_SP:
 795                 if (ncsi_priv->current_package == NCSI_PACKAGE_MAX)
 796                         ncsi_priv->current_package = 0;
 797                 ncsi_send_sp(ncsi_priv->current_package);
 798                 break;
 799         case NCSI_PROBE_PACKAGE_DP:
 800                 ncsi_send_dp(ncsi_priv->current_package);
 801                 break;
 802         case NCSI_PROBE_CHANNEL_SP:
 803                 if (ncsi_priv->n_packages > 0)
 804                         ncsi_send_sp(ncsi_priv->current_package);
 805                 else
 806                         printf("NCSI: no packages discovered, configuration not possible\n");
 807                 break;
 808         case NCSI_PROBE_CHANNEL:
 809                 /* Kicks off chain of channel discovery */
 810                 ncsi_send_cis(ncsi_priv->current_package,
 811                               ncsi_priv->current_channel);
 812                 break;
 813         case NCSI_CONFIG:
 814                 for (np = 0; np < ncsi_priv->n_packages; np++) {
 815                         package = &ncsi_priv->packages[np];
 816                         for (nc = 0; nc < package->n_channels; nc++)
 817                                 if (package->channels[nc].has_link)
 818                                         break;
 819                         if (nc < package->n_channels)
 820                                 break;
 821                 }
 822                 if (np == ncsi_priv->n_packages) {
 823                         printf("NCSI: no link available\n");
 824                         return;
 825                 }
 826
 827                 printf("NCSI: configuring channel %d\n", nc);
 828                 ncsi_priv->current_package = np;
 829                 ncsi_priv->current_channel = nc;
 830                 /* Kicks off rest of configure chain */
 831                 ncsi_send_sma(np, nc);
 832                 break;
 833         default:
 834                 printf("NCSI: unknown state 0x%x\n", ncsi_priv->state);
 835         }
 836 }
 837
 838 int ncsi_probe(struct phy_device *phydev)
 839 {
 840         if (!phydev->priv) {
 841                 phydev->priv = malloc(sizeof(struct ncsi));
 842                 if (!phydev->priv)
 843                         return -ENOMEM;
 844                 memset(phydev->priv, 0, sizeof(struct ncsi));
 845         }
 846
 847         ncsi_priv = phydev->priv;
 848
 849         return 0;
 850 }
 851
 852 int ncsi_startup(struct phy_device *phydev)
 853 {
 854         /* Set phydev parameters */
 855         phydev->speed = SPEED_100;
 856         phydev->duplex = DUPLEX_FULL;
 857         /* Normal phy reset is N/A */
 858         phydev->flags |= PHY_FLAG_BROKEN_RESET;
 859
 860         /* Set initial probe state */
 861         ncsi_priv->state = NCSI_PROBE_PACKAGE_SP;
 862
 863         /* No active package/channel yet */
 864         ncsi_priv->current_package = NCSI_PACKAGE_MAX;
 865         ncsi_priv->current_channel = NCSI_CHANNEL_MAX;
 866
 867         /* Pretend link works so the MAC driver sets final bits up */
 868         phydev->link = true;
 869
 870         /* Set ncsi_priv so we can use it when called from net_loop() */
 871         ncsi_priv = phydev->priv;
 872
 873         return 0;
 874 }
 875
 876 int ncsi_shutdown(struct phy_device *phydev)
 877 {
 878         printf("NCSI: Disabling package %d\n", ncsi_priv->current_package);
 879         ncsi_send_dp(ncsi_priv->current_package);
 880         return 0;
 881 }
 882
 883 static struct phy_driver ncsi_driver = {
 884         .uid            = PHY_NCSI_ID,
 885         .mask           = 0xffffffff,
 886         .name           = "NC-SI",
 887         .features       = PHY_100BT_FEATURES | PHY_DEFAULT_FEATURES |
 888                                 SUPPORTED_100baseT_Full | SUPPORTED_MII,
 889         .probe          = ncsi_probe,
 890         .startup        = ncsi_startup,
 891         .shutdown       = ncsi_shutdown,
 892 };
 893
 894 int phy_ncsi_init(void)
 895 {
 896         phy_register(&ncsi_driver);
 897         return 0;
 898 }