git.sesse.net Git - pistorm/blob - a314/software-amiga/ethernet_pistorm/device-2.c

   1 /*
   2  * Copyright (c) 2020-2021 Niklas Ekström
   3  *
   4  * Thanks to Christian Vogelgsang and Mike Sterling for inspiration gained from their SANA-II drivers:
   5  * - https://github.com/cnvogelg/plipbox
   6  * - https://github.com/mikestir/k1208-drivers
   7  */
   8
   9 #include <exec/resident.h>
  10 #include <exec/errors.h>
  11 #include <exec/memory.h>
  12 #include <exec/lists.h>
  13 #include <exec/alerts.h>
  14 #include <exec/devices.h>
  15 #include <exec/types.h>
  16 #include <exec/tasks.h>
  17 #include <exec/io.h>
  18 #include <exec/execbase.h>
  19 #include <exec/ports.h>
  20
  21 #include <libraries/expansion.h>
  22 #include <libraries/dos.h>
  23
  24 #include <devices/trackdisk.h>
  25 #include <devices/timer.h>
  26 #include <devices/scsidisk.h>
  27
  28 #include <dos/filehandler.h>
  29
  30 #include <proto/disk.h>
  31 #include <proto/expansion.h>
  32 #include <proto/exec.h>
  33 #include <proto/dos.h>
  34
  35 #include <string.h>
  36
  37 #include <clib/alib_protos.h>
  38
  39 #include "../../a314device/a314.h"
  40 #include "../../a314device/proto_a314.h"
  41 #include "sana2.h"
  42
  43 #include <stdint.h>
  44
  45 // Defines.
  46
  47 #define kprintf(...)
  48
  49 #define STR(s) #s
  50 #define XSTR(s) STR(s)
  51
  52 #define DEVICE_NAME "a314eth.device"
  53 #define DEVICE_DATE "(19 May 2021)"
  54 #define SERVICE_NAME "ethernet"
  55 #define DEVICE_ID_STRING "A314Eth " XSTR(DEVICE_VERSION) "." XSTR(DEVICE_REVISION) " " DEVICE_DATE
  56 #define DEVICE_VERSION 42
  57 #define DEVICE_REVISION 2
  58 #define DEVICE_PRIORITY 0
  59
  60
  61 #define DEVICE_NAME             "a314eth.device"
  62 #define SERVICE_NAME            "ethernet"
  63
  64 #define TASK_PRIO               10
  65
  66 #define MACADDR_SIZE            6
  67 #define NIC_BPS                 10000000
  68
  69 #define ETH_MTU                 1500
  70 #define RAW_MTU                 1518
  71
  72 #define READ_FRAME_REQ          1
  73 #define WRITE_FRAME_REQ         2
  74 #define READ_FRAME_RES          3
  75 #define WRITE_FRAME_RES         4
  76
  77 #define ET_RBUF_CNT             2
  78 #define ET_WBUF_CNT             2
  79 #define ET_BUF_CNT              (ET_RBUF_CNT + ET_WBUF_CNT)
  80
  81 int __attribute__((no_reorder)) _start()
  82 {
  83     return -1;
  84 }
  85
  86 asm("romtag:                                \n"
  87     "       dc.w    "XSTR(RTC_MATCHWORD)"   \n"
  88     "       dc.l    romtag                  \n"
  89     "       dc.l    endcode                 \n"
  90     "       dc.b    "XSTR(RTF_AUTOINIT)"    \n"
  91     "       dc.b    "XSTR(DEVICE_VERSION)"  \n"
  92     "       dc.b    "XSTR(NT_DEVICE)"       \n"
  93     "       dc.b    "XSTR(DEVICE_PRIORITY)" \n"
  94     "       dc.l    _device_name            \n"
  95     "       dc.l    _device_id_string       \n"
  96     "       dc.l    _auto_init_tables       \n"
  97     "endcode:                               \n"
  98         ".align 4\n"
  99         "       dc.l    16\n"
 100         "_device_process_seglist:\n"
 101         "       dc.l    0\n"
 102         "       jmp     _device_process_run\n");
 103
 104 char device_name[] = DEVICE_NAME;
 105 char device_id_string[] = DEVICE_ID_STRING;
 106
 107 // Typedefs.
 108
 109 typedef BOOL (*buf_copy_func_t)(void *dst asm("a0"), void *src asm("a1"), LONG size asm("d0"));
 110
 111 // Structs.
 112
 113 #pragma pack(push, 1)
 114 struct EthHdr
 115 {
 116         unsigned char eh_Dst[MACADDR_SIZE];
 117         unsigned char eh_Src[MACADDR_SIZE];
 118         unsigned short eh_Type;
 119 };
 120 #pragma pack(pop)
 121
 122 #pragma pack(push, 1)
 123 struct ServiceMsg
 124 {
 125         ULONG sm_Address;
 126         UWORD sm_Length;
 127         UWORD sm_Kind;
 128 };
 129 #pragma pack(pop)
 130
 131 struct BufDesc
 132 {
 133         struct MinNode bd_Node;
 134         void *bd_Buffer;
 135         int bd_Length;
 136 };
 137
 138 // Constants.
 139
 140 static const char service_name[] = SERVICE_NAME;
 141 static const char a314_device_name[] = A314_NAME;
 142
 143 static const unsigned char macaddr[MACADDR_SIZE] = { 0x40, 0x61, 0x33, 0x31, 0x34, 0x65 };
 144
 145 // Global variables.
 146
 147 BPTR saved_seg_list;
 148 struct ExecBase *SysBase;
 149 struct DosLibrary *DOSBase;
 150 struct Library *A314Base;
 151
 152 buf_copy_func_t copyfrom;
 153 buf_copy_func_t copyto;
 154
 155 volatile struct List ut_rbuf_list;
 156 volatile struct List ut_wbuf_list;
 157
 158 struct BufDesc et_bufs[ET_BUF_CNT];
 159
 160 struct List et_rbuf_free_list;
 161 struct List et_rbuf_pending_list;
 162 struct List et_rbuf_has_data_list;
 163
 164 struct List et_wbuf_free_list;
 165 struct List et_wbuf_pending_list;
 166
 167 LONG a314_socket;
 168 short last_req_kind;
 169
 170 struct MsgPort a314_mp;
 171
 172 struct A314_IORequest read_ior;
 173 struct A314_IORequest write_ior;
 174 struct A314_IORequest reset_ior;
 175
 176 BOOL pending_a314_read;
 177 BOOL pending_a314_write;
 178 BOOL pending_a314_reset;
 179
 180 struct ServiceMsg a314_read_buf;
 181 struct ServiceMsg a314_write_buf;
 182
 183 volatile ULONG sana2_sigmask;
 184 volatile ULONG shutdown_sigmask;
 185
 186 volatile struct Task *init_task;
 187
 188 struct Process *device_process;
 189 volatile int device_start_error;
 190 void device_process_run();
 191
 192 // External declarations.
 193
 194 extern void device_process_seglist();
 195
 196 // Procedures.
 197
 198 static struct Library __attribute__((used)) *init_device(uint8_t *seg_list asm("a0"), struct Library *dev asm("d0"))
 199 {
 200         saved_seg_list = (BPTR)seg_list;
 201
 202         dev->lib_Node.ln_Type = NT_DEVICE;
 203         dev->lib_Node.ln_Name = (char *)device_name;
 204         dev->lib_Flags = LIBF_SUMUSED | LIBF_CHANGED;
 205         dev->lib_Version = 1;
 206         dev->lib_Revision = 0;
 207         dev->lib_IdString = (APTR)device_id_string;
 208
 209         SysBase = *(struct ExecBase **)4;
 210         DOSBase = (struct DosLibrary *)OpenLibrary((STRPTR)DOSNAME, 0);
 211
 212         return dev;
 213 }
 214
 215 static uint8_t* __attribute__((used)) expunge(struct Library *dev asm("a6"))
 216 {
 217         if (dev->lib_OpenCnt)
 218         {
 219                 dev->lib_Flags |= LIBF_DELEXP;
 220                 return 0;
 221         }
 222
 223         // Shady way of waiting for device process to terminate before unloading.
 224         Delay(10);
 225
 226         CloseLibrary((struct Library *)DOSBase);
 227
 228         Remove(&dev->lib_Node);
 229         FreeMem((char *)dev - dev->lib_NegSize, dev->lib_NegSize + dev->lib_PosSize);
 230         return (uint8_t *)saved_seg_list;
 231 }
 232
 233 static void send_a314_cmd(struct A314_IORequest *ior, UWORD cmd, char *buffer, int length)
 234 {
 235         ior->a314_Request.io_Command = cmd;
 236         ior->a314_Request.io_Error = 0;
 237         ior->a314_Socket = a314_socket;
 238         ior->a314_Buffer = (STRPTR)buffer;
 239         ior->a314_Length = length;
 240         SendIO((struct IORequest *)ior);
 241 }
 242
 243 static void do_a314_cmd(struct A314_IORequest *ior, UWORD cmd, char *buffer, int length)
 244 {
 245         ior->a314_Request.io_Command = cmd;
 246         ior->a314_Request.io_Error = 0;
 247         ior->a314_Socket = a314_socket;
 248         ior->a314_Buffer = (STRPTR)buffer;
 249         ior->a314_Length = length;
 250         DoIO((struct IORequest *)ior);
 251 }
 252
 253 static void copy_from_bd_and_reply(struct IOSana2Req *ios2, struct BufDesc *bd)
 254 {
 255         struct EthHdr *eh = bd->bd_Buffer;
 256
 257         if (ios2->ios2_Req.io_Flags & SANA2IOF_RAW)
 258         {
 259                 ios2->ios2_DataLength = bd->bd_Length;
 260                 copyto(ios2->ios2_Data, bd->bd_Buffer, ios2->ios2_DataLength);
 261                 ios2->ios2_Req.io_Flags = SANA2IOF_RAW;
 262         }
 263         else
 264         {
 265                 ios2->ios2_DataLength = bd->bd_Length - sizeof(struct EthHdr);
 266                 copyto(ios2->ios2_Data, &eh[1], ios2->ios2_DataLength);
 267                 ios2->ios2_Req.io_Flags = 0;
 268         }
 269
 270         memcpy(ios2->ios2_SrcAddr, eh->eh_Src, MACADDR_SIZE);
 271         memcpy(ios2->ios2_DstAddr, eh->eh_Dst, MACADDR_SIZE);
 272
 273         BOOL bcast = TRUE;
 274         for (int i = 0; i < MACADDR_SIZE; i++)
 275         {
 276                 if (eh->eh_Dst[i] != 0xff)
 277                 {
 278                         bcast = FALSE;
 279                         break;
 280                 }
 281         }
 282
 283         if (bcast)
 284                 ios2->ios2_Req.io_Flags |= SANA2IOF_BCAST;
 285
 286         ios2->ios2_PacketType = eh->eh_Type;
 287
 288         ios2->ios2_Req.io_Error = 0;
 289         ReplyMsg(&ios2->ios2_Req.io_Message);
 290 }
 291
 292 static void copy_to_bd_and_reply(struct BufDesc *bd, struct IOSana2Req *ios2)
 293 {
 294         struct EthHdr *eh = bd->bd_Buffer;
 295
 296         if (ios2->ios2_Req.io_Flags & SANA2IOF_RAW)
 297         {
 298                 copyfrom(bd->bd_Buffer, ios2->ios2_Data, ios2->ios2_DataLength);
 299                 bd->bd_Length = ios2->ios2_DataLength;
 300         }
 301         else
 302         {
 303                 eh->eh_Type = ios2->ios2_PacketType;
 304                 memcpy(eh->eh_Src, macaddr, sizeof(macaddr));
 305                 memcpy(eh->eh_Dst, ios2->ios2_DstAddr, MACADDR_SIZE);
 306                 copyfrom(&eh[1], ios2->ios2_Data, ios2->ios2_DataLength);
 307                 bd->bd_Length = ios2->ios2_DataLength + sizeof(struct EthHdr);
 308         }
 309
 310         ios2->ios2_Req.io_Error = 0;
 311         ReplyMsg(&ios2->ios2_Req.io_Message);
 312 }
 313
 314 static void handle_a314_reply(struct A314_IORequest *ior)
 315 {
 316         if (ior == &write_ior)
 317         {
 318                 pending_a314_write = FALSE;
 319
 320                 if (ior->a314_Request.io_Error == A314_WRITE_OK)
 321                 {
 322                         // Start new write later.
 323                 }
 324                 else // A314_WRITE_RESET
 325                 {
 326                         // TODO: Handle. What if pi-side is shutting down.
 327                 }
 328         }
 329         else if (ior == &read_ior)
 330         {
 331                 pending_a314_read = FALSE;
 332
 333                 if (ior->a314_Request.io_Error == A314_READ_OK)
 334                 {
 335                         if (a314_read_buf.sm_Kind == WRITE_FRAME_RES)
 336                         {
 337                                 struct BufDesc *bd = (struct BufDesc *)RemHead(&et_wbuf_pending_list);
 338                                 AddTail(&et_wbuf_free_list, (struct Node *)bd);
 339                         }
 340                         else // READ_FRAME_RES
 341                         {
 342                                 struct BufDesc *bd = (struct BufDesc *)RemHead(&et_rbuf_pending_list);
 343                                 bd->bd_Length = a314_read_buf.sm_Length;
 344                                 AddTail(&et_rbuf_has_data_list, (struct Node *)bd);
 345                         }
 346
 347                         send_a314_cmd(&read_ior, A314_READ, (void *)&a314_read_buf, sizeof(a314_read_buf));
 348                         pending_a314_read = TRUE;
 349                 }
 350                 else // A314_READ_RESET
 351                 {
 352                         // TODO: Handle. What if pi-side is shutting down.
 353                 }
 354         }
 355         else if (ior == &reset_ior)
 356         {
 357                 pending_a314_reset = FALSE;
 358         }
 359 }
 360
 361 static struct IOSana2Req *remove_matching_rbuf(ULONG type)
 362 {
 363         struct Node *node = ut_rbuf_list.lh_Head;
 364         while (node->ln_Succ)
 365         {
 366                 struct IOSana2Req *ios2 = (struct IOSana2Req *)node;
 367                 if (ios2->ios2_PacketType == type)
 368                 {
 369                         Remove(node);
 370                         return ios2;
 371                 }
 372                 node = node->ln_Succ;
 373         }
 374         return NULL;
 375 }
 376
 377 static void complete_read_reqs()
 378 {
 379         struct Node *node = et_rbuf_has_data_list.lh_Head;
 380         if (!node->ln_Succ)
 381                 return;
 382
 383         Forbid();
 384         while (node->ln_Succ)
 385         {
 386                 struct BufDesc *bd = (struct BufDesc *)node;
 387                 struct EthHdr *eh = (struct EthHdr *)bd->bd_Buffer;
 388
 389                 node = node->ln_Succ;
 390
 391                 struct IOSana2Req *ios2 = remove_matching_rbuf(eh->eh_Type);
 392                 if (ios2)
 393                 {
 394                         copy_from_bd_and_reply(ios2, bd);
 395
 396                         Remove((struct Node *)bd);
 397                         AddTail(&et_rbuf_free_list, (struct Node *)bd);
 398                 }
 399         }
 400         Permit();
 401 }
 402
 403 static void maybe_write_req()
 404 {
 405         if (pending_a314_write)
 406                 return;
 407
 408         BOOL free_et_wbuf = et_wbuf_free_list.lh_Head->ln_Succ != NULL;
 409         BOOL idle_et_rbuf = et_rbuf_free_list.lh_Head->ln_Succ != NULL;
 410
 411         Forbid();
 412
 413         BOOL waiting_ut_wbuf = ut_wbuf_list.lh_Head->ln_Succ != NULL;
 414
 415         BOOL want_wbuf = free_et_wbuf && waiting_ut_wbuf;
 416         BOOL want_rbuf = idle_et_rbuf;
 417
 418         if (!want_rbuf && !want_wbuf)
 419         {
 420                 Permit();
 421                 return;
 422         }
 423
 424         short next_req_kind = 0;
 425
 426         if (last_req_kind == WRITE_FRAME_REQ)
 427                 next_req_kind = want_rbuf ? READ_FRAME_REQ : WRITE_FRAME_REQ;
 428         else
 429                 next_req_kind = want_wbuf ? WRITE_FRAME_REQ : READ_FRAME_REQ;
 430
 431         struct IOSana2Req *ios2 = NULL;
 432         if (next_req_kind == WRITE_FRAME_REQ)
 433                 ios2 = (struct IOSana2Req*)RemHead((struct List *)&ut_wbuf_list);
 434
 435         Permit();
 436
 437         struct BufDesc *bd;
 438
 439         if (next_req_kind == READ_FRAME_REQ)
 440         {
 441                 bd = (struct BufDesc *)RemHead(&et_rbuf_free_list);
 442                 bd->bd_Length = RAW_MTU;
 443                 AddTail(&et_rbuf_pending_list, (struct Node *)&bd->bd_Node);
 444         }
 445         else // WRITE_FRAME_REQ
 446         {
 447                 bd = (struct BufDesc *)RemHead(&et_wbuf_free_list);
 448                 copy_to_bd_and_reply(bd, ios2);
 449                 AddTail(&et_wbuf_pending_list, (struct Node *)bd);
 450         }
 451
 452         a314_write_buf.sm_Address = TranslateAddressA314(bd->bd_Buffer);
 453         a314_write_buf.sm_Length = bd->bd_Length;
 454         a314_write_buf.sm_Kind = next_req_kind;
 455
 456         send_a314_cmd(&write_ior, A314_WRITE, (void *)&a314_write_buf, sizeof(a314_write_buf));
 457         pending_a314_write = TRUE;
 458
 459         last_req_kind = next_req_kind;
 460 }
 461
 462 void device_process_run()
 463 {
 464         ULONG sana2_signal = AllocSignal(-1);
 465         sana2_sigmask = 1UL << sana2_signal;
 466
 467         ULONG shutdown_signal = AllocSignal(-1);
 468         shutdown_sigmask = 1UL << shutdown_signal;
 469
 470         a314_mp.mp_SigBit = AllocSignal(-1);
 471         a314_mp.mp_SigTask = FindTask(NULL);
 472
 473         do_a314_cmd(&reset_ior, A314_CONNECT, (char *)service_name, strlen(service_name));
 474         device_start_error = reset_ior.a314_Request.io_Error == A314_CONNECT_OK ? 0 : -1;
 475
 476         Signal((struct Task *)init_task, SIGF_SINGLE);
 477
 478         if (device_start_error)
 479                 return;
 480
 481         ULONG a314_sigmask = 1UL << a314_mp.mp_SigBit;
 482
 483         send_a314_cmd(&read_ior, A314_READ, (void *)&a314_read_buf, sizeof(a314_read_buf));
 484         pending_a314_read = TRUE;
 485
 486         BOOL shutting_down = FALSE;
 487
 488         while (TRUE)
 489         {
 490                 complete_read_reqs();
 491                 maybe_write_req();
 492
 493                 if (shutting_down && !pending_a314_read && !pending_a314_write && !pending_a314_reset)
 494                         break;
 495
 496                 ULONG sigs = Wait(a314_sigmask | sana2_sigmask | shutdown_sigmask);
 497
 498                 if ((sigs & shutdown_sigmask) && !shutting_down)
 499                 {
 500                         send_a314_cmd(&reset_ior, A314_RESET, NULL, 0);
 501                         pending_a314_reset = TRUE;
 502                         shutting_down = TRUE;
 503                 }
 504
 505                 if (sigs & a314_sigmask)
 506                 {
 507                         struct A314_IORequest *ior;
 508                         while ((ior = (struct A314_IORequest *)GetMsg(&a314_mp)))
 509                                 handle_a314_reply(ior);
 510                 }
 511         }
 512
 513         Signal((struct Task *)init_task, SIGF_SINGLE);
 514 }
 515
 516 static struct TagItem *FindTagItem(Tag tagVal, struct TagItem *tagList)
 517 {
 518         struct TagItem *ti = tagList;
 519         while (ti && ti->ti_Tag != tagVal)
 520         {
 521                 switch (ti->ti_Tag)
 522                 {
 523                 case TAG_DONE:
 524                         return NULL;
 525                 case TAG_MORE:
 526                         ti = (struct TagItem *)ti->ti_Data;
 527                         break;
 528                 case TAG_SKIP:
 529                         ti += ti->ti_Data + 1;
 530                         break;
 531                 case TAG_IGNORE:
 532                 default:
 533                         ti++;
 534                         break;
 535                 }
 536         }
 537         return ti;
 538 }
 539
 540 static ULONG GetTagData(Tag tagVal, ULONG defaultData, struct TagItem *tagList)
 541 {
 542         struct TagItem *ti = FindTagItem(tagVal, tagList);
 543         return ti ? ti->ti_Data : defaultData;
 544 }
 545
 546 static void __attribute__((used)) open(struct Library *dev asm("a6"), struct IOSana2Req *ios2 asm("a1"), uint32_t unitnum asm("d0"), uint32_t flags asm("d1"))
 547 {
 548         kprintf("We opening this shit.\n");
 549         ios2->ios2_Req.io_Error = IOERR_OPENFAIL;
 550         ios2->ios2_Req.io_Message.mn_Node.ln_Type = NT_REPLYMSG;
 551
 552         if (unitnum != 0 || dev->lib_OpenCnt)
 553                 return;
 554
 555         dev->lib_OpenCnt++;
 556
 557         kprintf("Try the copyfrom crap.\n");
 558         copyfrom = (buf_copy_func_t)GetTagData(S2_CopyFromBuff, 0, (struct TagItem *)ios2->ios2_BufferManagement);
 559         copyto = (buf_copy_func_t)GetTagData(S2_CopyToBuff, 0, (struct TagItem *)ios2->ios2_BufferManagement);
 560         ios2->ios2_BufferManagement = (void *)0xdeadbeefUL;
 561
 562         kprintf("Memsetting some shit.\n");
 563         memset(&a314_mp, 0, sizeof(a314_mp));
 564         a314_mp.mp_Node.ln_Pri = 0;
 565         a314_mp.mp_Node.ln_Type = NT_MSGPORT;
 566         a314_mp.mp_Node.ln_Name = (char *)device_name;
 567         a314_mp.mp_Flags = PA_SIGNAL;
 568         NewList(&a314_mp.mp_MsgList);
 569
 570         kprintf("Memsetting more shit.\n");
 571         memset(&write_ior, 0, sizeof(write_ior));
 572         write_ior.a314_Request.io_Message.mn_ReplyPort = &a314_mp;
 573         write_ior.a314_Request.io_Message.mn_Length = sizeof(write_ior);
 574         write_ior.a314_Request.io_Message.mn_Node.ln_Type = NT_REPLYMSG;
 575
 576         kprintf("Opendevice.\n");
 577         A314Base = NULL;
 578         if (OpenDevice((STRPTR)a314_device_name, 0, (struct IORequest *)&write_ior, 0))
 579                 goto error;
 580
 581         A314Base = &(write_ior.a314_Request.io_Device->dd_Library);
 582
 583         kprintf("Copy memory.\n");
 584         memcpy(&read_ior, &write_ior, sizeof(read_ior));
 585         memcpy(&reset_ior, &write_ior, sizeof(reset_ior));
 586
 587         kprintf("Making datestamps.\n");
 588         struct DateStamp ds;
 589         DateStamp(&ds);
 590         a314_socket = (ds.ds_Minute * 60 * TICKS_PER_SECOND) + ds.ds_Tick;
 591
 592         last_req_kind = WRITE_FRAME_REQ;
 593
 594         kprintf("Making lists.\n");
 595         NewList((struct List *)&ut_rbuf_list);
 596         NewList((struct List *)&ut_wbuf_list);
 597
 598         NewList(&et_rbuf_free_list);
 599         NewList(&et_rbuf_pending_list);
 600         NewList(&et_rbuf_has_data_list);
 601
 602         NewList(&et_wbuf_free_list);
 603         NewList(&et_wbuf_pending_list);
 604
 605         kprintf("Memzero buffers.\n");
 606         for (int i = 0; i < ET_BUF_CNT; i++)
 607                 memset(&et_bufs[i], 0, sizeof(struct BufDesc));
 608
 609         kprintf("Add tails.\n");
 610         for (int i = 0; i < ET_BUF_CNT; i++)
 611         {
 612                 struct BufDesc *bd = &et_bufs[i];
 613
 614                 bd->bd_Buffer = AllocMem(RAW_MTU, MEMF_FAST);
 615                 if (!bd->bd_Buffer)
 616                         goto error;
 617
 618                 if (i < ET_RBUF_CNT)
 619                         AddTail(&et_rbuf_free_list, (struct Node*)&bd->bd_Node);
 620                 else
 621                         AddTail(&et_wbuf_free_list, (struct Node*)&bd->bd_Node);
 622         }
 623
 624         kprintf("Find task.\n");
 625         init_task = FindTask(NULL);
 626
 627         kprintf("Do msgport.\n");
 628         struct MsgPort *device_mp = CreateProc((STRPTR)device_name, TASK_PRIO, ((ULONG)&device_process_seglist) >> 2, 2048);
 629         if (!device_mp)
 630                 goto error;
 631
 632         kprintf("Process thing.\n");
 633         device_process = (struct Process *)((char *)device_mp - sizeof(struct Task));
 634
 635         kprintf("Waitf.\n");
 636         Wait(SIGF_SINGLE);
 637         kprintf("Waitedf.\n");
 638
 639         if (device_start_error) {
 640                 kprintf("Device start error.\n");
 641                 goto error;
 642         }
 643
 644         kprintf("Everything ok?\n");
 645         ios2->ios2_Req.io_Error = 0;
 646         return;
 647
 648 error:
 649         kprintf("Error small farts.\n");
 650         for (int i = ET_BUF_CNT - 1; i >= 0; i--)
 651                 if (et_bufs[i].bd_Buffer)
 652                         FreeMem(et_bufs[i].bd_Buffer, RAW_MTU);
 653
 654         if (A314Base)
 655         {
 656                 CloseDevice((struct IORequest *)&write_ior);
 657                 A314Base = NULL;
 658         }
 659
 660         dev->lib_OpenCnt--;
 661 }
 662
 663 static uint8_t* __attribute__((used)) close(struct Library *dev asm("a6"), struct IOSana2Req *ios2 asm("a1"))
 664 {
 665         kprintf("Close.\n");
 666         init_task = FindTask(NULL);
 667         Signal(&device_process->pr_Task, shutdown_sigmask);
 668         Wait(SIGF_SINGLE);
 669
 670         for (int i = ET_BUF_CNT - 1; i >= 0; i--)
 671                 FreeMem(et_bufs[i].bd_Buffer, RAW_MTU);
 672
 673         CloseDevice((struct IORequest *)&write_ior);
 674         A314Base = NULL;
 675
 676         ios2->ios2_Req.io_Device = NULL;
 677         ios2->ios2_Req.io_Unit = NULL;
 678
 679         dev->lib_OpenCnt--;
 680
 681         if (dev->lib_OpenCnt == 0 && (dev->lib_Flags & LIBF_DELEXP))
 682                 return expunge(dev);
 683
 684         return 0;
 685 }
 686
 687 static void device_query(struct IOSana2Req *req)
 688 {
 689         struct Sana2DeviceQuery *query;
 690
 691         query = req->ios2_StatData;
 692         query->DevQueryFormat = 0;
 693         query->DeviceLevel = 0;
 694
 695         if (query->SizeAvailable >= 18)
 696                 query->AddrFieldSize = MACADDR_SIZE * 8;
 697
 698         if (query->SizeAvailable >= 22)
 699                 query->MTU = ETH_MTU;
 700
 701         if (query->SizeAvailable >= 26)
 702                 query->BPS = NIC_BPS;
 703
 704         if (query->SizeAvailable >= 30)
 705                 query->HardwareType = S2WireType_Ethernet;
 706
 707         query->SizeSupplied = query->SizeAvailable < 30 ? query->SizeAvailable : 30;
 708 }
 709
 710 static void __attribute__((used)) begin_io(struct Library *dev asm("a6"), struct IOSana2Req *ios2 asm("a1"))
 711 {
 712         kprintf("BeginIO.\n");
 713         ios2->ios2_Req.io_Error = S2ERR_NO_ERROR;
 714         ios2->ios2_WireError = S2WERR_GENERIC_ERROR;
 715
 716         switch (ios2->ios2_Req.io_Command)
 717         {
 718         case CMD_READ:
 719                 if (!ios2->ios2_BufferManagement)
 720                 {
 721                         ios2->ios2_Req.io_Error = S2ERR_BAD_ARGUMENT;
 722                         ios2->ios2_WireError = S2WERR_BUFF_ERROR;
 723                         break;
 724                 }
 725
 726                 Forbid();
 727                 AddTail((struct List *)&ut_rbuf_list, &ios2->ios2_Req.io_Message.mn_Node);
 728                 Permit();
 729
 730                 ios2->ios2_Req.io_Flags &= ~SANA2IOF_QUICK;
 731                 ios2 = NULL;
 732
 733                 Signal(&device_process->pr_Task, sana2_sigmask);
 734                 break;
 735
 736         case S2_BROADCAST:
 737                 memset(ios2->ios2_DstAddr, 0xff, MACADDR_SIZE);
 738                 /* Fall through */
 739
 740         case CMD_WRITE:
 741                 if (((ios2->ios2_Req.io_Flags & SANA2IOF_RAW) != 0 && ios2->ios2_DataLength > RAW_MTU) ||
 742                         ((ios2->ios2_Req.io_Flags & SANA2IOF_RAW) == 0 && ios2->ios2_DataLength > ETH_MTU))
 743                 {
 744                         ios2->ios2_Req.io_Error = S2ERR_MTU_EXCEEDED;
 745                         break;
 746                 }
 747
 748                 if (!ios2->ios2_BufferManagement)
 749                 {
 750                         ios2->ios2_Req.io_Error = S2ERR_BAD_ARGUMENT;
 751                         ios2->ios2_WireError = S2WERR_BUFF_ERROR;
 752                         break;
 753                 }
 754
 755                 Forbid();
 756                 AddTail((struct List *)&ut_wbuf_list, &ios2->ios2_Req.io_Message.mn_Node);
 757                 Permit();
 758
 759                 ios2->ios2_Req.io_Flags &= ~SANA2IOF_QUICK;
 760                 ios2 = NULL;
 761
 762                 Signal(&device_process->pr_Task, sana2_sigmask);
 763                 break;
 764
 765         case S2_ONLINE:
 766         case S2_OFFLINE:
 767         case S2_CONFIGINTERFACE:
 768                 break;
 769         case S2_GETSTATIONADDRESS:
 770                 memcpy(ios2->ios2_SrcAddr, macaddr, sizeof(macaddr));
 771                 memcpy(ios2->ios2_DstAddr, macaddr, sizeof(macaddr));
 772                 break;
 773         case S2_DEVICEQUERY:
 774                 device_query(ios2);
 775                 break;
 776
 777         case S2_ONEVENT:
 778         case S2_TRACKTYPE:
 779         case S2_UNTRACKTYPE:
 780         case S2_GETTYPESTATS:
 781         case S2_READORPHAN:
 782         case S2_GETGLOBALSTATS:
 783         case S2_GETSPECIALSTATS:
 784                 break;
 785
 786         default:
 787                 ios2->ios2_Req.io_Error = IOERR_NOCMD;
 788                 ios2->ios2_WireError = S2WERR_GENERIC_ERROR;
 789                 break;
 790         }
 791
 792         if (ios2)
 793         {
 794                 if (ios2->ios2_Req.io_Flags & SANA2IOF_QUICK)
 795                         ios2->ios2_Req.io_Message.mn_Node.ln_Type = NT_MESSAGE;
 796                 else
 797                         ReplyMsg(&ios2->ios2_Req.io_Message);
 798         }
 799 }
 800
 801 static void remove_from_list(struct List *list, struct Node *node)
 802 {
 803         for (struct Node *n = list->lh_Head; n->ln_Succ; n = n->ln_Succ)
 804         {
 805                 if (n == node)
 806                 {
 807                         Remove(n);
 808                         return;
 809                 }
 810         }
 811 }
 812
 813 static uint32_t __attribute__((used)) abort_io(struct Library *dev asm("a6"), struct IOSana2Req *ios2 asm("a1"))
 814 {
 815         kprintf("AbortIO.\n");
 816         Forbid();
 817         remove_from_list((struct List *)&ut_rbuf_list, &ios2->ios2_Req.io_Message.mn_Node);
 818         remove_from_list((struct List *)&ut_wbuf_list, &ios2->ios2_Req.io_Message.mn_Node);
 819         Permit();
 820
 821         ios2->ios2_Req.io_Error = IOERR_ABORTED;
 822         ios2->ios2_WireError = 0;
 823         ReplyMsg(&ios2->ios2_Req.io_Message);
 824
 825         return 0;
 826 }
 827
 828 static ULONG device_vectors[] =
 829 {
 830         (ULONG)open,
 831         (ULONG)close,
 832         (ULONG)expunge,
 833         0,
 834         (ULONG)begin_io,
 835         (ULONG)abort_io,
 836         -1,
 837 };
 838
 839 ULONG auto_init_tables[] =
 840 {
 841         sizeof(struct Library),
 842         (ULONG)device_vectors,
 843         0,
 844         (ULONG)init_device,
 845 };