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