Added semi-working bootloader

11 files changed, 501 insertions, 290 deletions

diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index a35533f..0a731e8 100644
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -13,7 +13,7 @@ jobs:
     steps:
       - uses: actions/checkout@v1
       - name: Install
-        run: sudo apt-get update && sudo apt-get install -y build-essential bison flex libgmp3-dev libmpc-dev libmpfr-dev texinfo libcloog-isl-dev libisl-0.18-dev curl nasm grub2 qemu qemu-kvm mtools
+        run: sudo apt-get update && sudo apt-get install -y build-essential bison flex libgmp3-dev libmpc-dev libmpfr-dev texinfo libcloog-isl-dev libisl-0.18-dev curl nasm genisoimage qemu qemu-kvm mtools
       - name: Build
         run: make cross build
       - name: Release
diff --git a/Makefile b/Makefile
index 6317d6b..81fc74e 100644
--- a/Makefile
+++ b/Makefile
@@ -26,7 +26,6 @@ build: clean
 		i686-elf-gcc -c ./"$${line}" -o ./build/"$${stripped}" -I ./src -std=gnu99 -ffreestanding -O3 -Wall -Wextra -Wno-unused-parameter || exit; \
 	done <./build/tmp; \
 	rm ./build/tmp; \
-
 	i686-elf-gcc -T ./src/kernel/linker.ld -I ./src -o ./build/melvix.bin -std=gnu99 -ffreestanding -O2 -nostdlib ./build/*.o || exit; \
 
 	# Testing
@@ -40,9 +39,7 @@ build: clean
 	# Create ISO
 	mkdir -p ./iso/boot/; \
 	mv ./build/melvix.bin ./iso/boot/kernel.bin; \
-	nasm ./src/bootloader/stage1.asm -f bin -o ./build/stage1.bin || exit; \
-	nasm ./src/bootloader/stage2.asm -f bin -o ./build/stage2.bin || exit; \
-	cat ./build/stage1.bin ./build/stage2.bin > ./iso/boot/boot.bin; \
+	nasm ./src/bootloader/loader.asm -f bin -o ./iso/boot/boot.bin || exit; \
 	genisoimage -no-emul-boot -b boot/boot.bin -o ./build/melvix.iso ./iso; \
 
 cross:
diff --git a/README.md b/README.md
index 64b9805..a9b6f27 100644
--- a/README.md
+++ b/README.md
@@ -25,7 +25,7 @@
   * texinfo
   * curl
   * nasm
-  * grub
+  * genisoimage
   * qemu
 
 * Build a cross compiler using `make cross`
diff --git a/src/bootloader/loader.asm b/src/bootloader/loader.asm
new file mode 100644
index 0000000..28e4562
--- /dev/null
+++ b/src/bootloader/loader.asm
@@ -0,0 +1,444 @@
+BITS 16
+ORG 0x7C00
+
+; Save the boot drive id
+mov [bootDriveID], dl
+
+jmp start
+
+; The print function
+print:
+    mov ah, 0x0E
+    xor bh, bh
+    .print_L:
+        lodsb
+        test al, al
+        jz .print_end
+        int 0x10
+        jmp .print_L
+    .print_end:
+    ret
+
+bootDriveID: db 0
+
+dapack:
+    dapack_size: db 0x10
+    dapack_null: db 0x00
+    dapack_blkcount: dw 0x0001
+    dapack_boffset: dw 0x9000
+    dapack_bsegment: dw 0x0000
+    dapack_start: dd 0x00000000
+    dapack_upper_lba_bits: dd 0x00000000
+
+readsector:
+    mov dword [dapack_start], eax
+    mov ah, 0x42
+    mov dl, [bootDriveID]
+    xor bx, bx
+    mov ds, bx
+    mov si, dapack
+    int 0x13
+    ret
+
+filenameLength dw 1
+filename dw 1
+
+findfile:
+    mov bx, [esp+2]
+    mov ax, [esp+4]
+    mov [filenameLength], ax
+    mov ax, [esp+6]
+    mov [filename], ax
+
+    .findfile_L:
+        mov al, [bx]
+
+        test al, al
+        jz .findfile_notfound
+
+        mov ah, [bx+32]
+        cmp ah, byte [filenameLength]
+        jnz .findfile_keep
+
+        call findfile_check
+        test ax, ax
+        jz findfile_found
+
+        .findfile_keep:
+        mov al, [bx]
+        xor ah, ah
+        add bx, ax
+        jmp .findfile_L
+
+    .findfile_notfound:
+        xor ax, ax
+        inc ax
+        ret
+
+    findfile_check:
+        pusha
+        add bx, 33
+        mov ax, bx
+
+        xor cx, cx
+        .findfile_check_L:
+            mov bx, [filename]
+            add bx, cx
+            mov dh, [bx]
+            mov bx, ax
+            add bx, cx
+            mov dl, [bx]
+            cmp dh, dl
+            jnz .findfile_check_fail
+
+            inc cx
+            cmp cx, word [filenameLength]
+            jz .findfile_check_success
+            jmp .findfile_check_L
+
+            .findfile_check_fail:
+                popa
+                xor ax, ax
+                inc ax
+                ret
+            .findfile_check_success:
+                popa
+                xor ax, ax
+                ret
+
+    findfile_found:
+        xor ax, ax
+        ret
+
+LOAD_PVD:
+    mov eax, 0x10
+    PVD_L:
+        call readsector
+        mov bx, [dapack_boffset]
+        mov bl, [bx]
+        cmp bl, 0x01
+        jz PVD_FOUND
+        inc eax
+        jmp PVD_L
+    PVD_FOUND:
+    ret
+
+checkA20:
+    ; Stolen from https://wiki.osdev.org/A20_Line
+    pushf
+    push ds
+    push es
+    push di
+    push si
+    cli
+    xor ax, ax
+    mov es, ax
+    not ax
+    mov ds, ax
+    mov di, 0x0500
+    mov si, 0x0510
+    mov al, byte [es:di]
+    push ax
+    mov al, byte [ds:si]
+    push ax
+    mov byte [es:di], 0x00
+    mov byte [ds:si], 0xFF
+    cmp byte [es:di], 0xFF
+    pop ax
+    mov byte [ds:si], al
+    pop ax
+    mov byte [es:di], al
+    mov ax, 0
+    jz checkA20_exit
+    mov ax, 1
+    checkA20_exit:
+        pop si
+        pop di
+        pop es
+        pop ds
+        popf
+        ret
+
+welcome db "Melvix", 0x0A, 0x0D, 0x00
+nolba db "BIOS lacks support for LBA addressing.", 0x00
+noboot db "Boot directory could not be found.", 0x00
+noa20 db "A20 could not be enabled.", 0
+loading db "Loading kernel...", 0x0A, 0x0D, 0x00
+nokernel db "kernel.bin could not be found!", 0
+booting db "Booting...", 0x0A, 0x0D, 0x00
+nomem db "BIOS does not support memory detection!", 0
+memno20 db "BIOS returns memory detection with 24 bytes. This has never been seen!", 0
+
+start:
+    ; Check if LBA is supported by the BIOS.
+    mov ah, 0x41
+    mov bx, 0x55AA
+    int 0x13
+    jc lba_not_supported
+    cmp bx, 0xAA55
+    jnz lba_not_supported
+
+    call checkA20
+    test ax, ax
+    jnz A20_ENABLED
+
+    in al, 0x92
+    or al, 2
+    out 0x92, al
+
+    call checkA20
+    test ax, ax
+    jnz A20_ENABLED
+
+    mov si, noa20
+    call print
+    jmp $
+
+A20_ENABLED:
+    ; Inspired by https://wiki.osdev.org/Unreal_Mode
+    cli
+    push ds
+    lgdt [gdtinfo]
+
+    mov eax, cr0
+    or al, 1
+    mov cr0, eax
+
+    jmp $+2
+
+    mov bx, 0x08
+    mov ds, bx
+
+    and al, 0xFE
+    mov cr0, eax
+    pop ds
+
+    mov si, loading
+    call print
+
+    call LOAD_PVD
+
+    mov bx, 0x9000
+    add bx, 156
+    add bx, 2
+    mov eax, dword [bx]
+    call readsector
+
+    push boot
+    push boot_len
+    push 0x9000
+    call findfile
+    add esp, 6
+    test ax, ax
+    jz continue_BOOT
+
+    mov si, noboot
+    call print
+    jmp $
+
+continue_BOOT:
+    mov word [dapack_blkcount], 0x0001
+    add bx, 2
+    mov eax, [bx]
+    call readsector
+
+    push kernelbin
+    push kernelbin_len
+    push 0x9000
+    call findfile
+    add esp, 6
+    test ax, ax
+    jz continue_KERNEL
+
+    mov si, nokernel
+    call print
+    jmp $
+
+continue_KERNEL:
+    mov word [dapack_blkcount], 1
+    mov word [dapack_boffset], 0x9000
+
+    push bx
+    add bx, 10
+    mov eax, [bx]
+    xor edx, edx
+    mov ebx, 2048
+    div ebx
+    inc eax
+    pop bx
+    push eax
+
+    add bx, 2
+    mov eax, [bx]
+    push eax
+
+    ; Offsetless count.
+    xor ecx, ecx
+LOAD_KERNEL:
+    mov eax, [esp]
+    add eax, ecx
+    call readsector
+
+    mov eax, ecx
+    shl eax, 11
+    add eax, 0x200000
+
+    push ecx
+    xor ecx, ecx
+    .LOAD_KERNEL_L:
+        mov ebx, ecx
+        add ebx, 0x9000
+        mov ebx, dword [ebx]
+
+        mov edx, ebx
+        mov ebx, eax
+        add ebx, ecx
+        mov dword [ebx], edx
+
+        add ecx, 4
+        cmp ecx, 2048
+        jl .LOAD_KERNEL_L
+    pop ecx
+
+    inc ecx
+    cmp ecx, dword [esp+4]
+    jl LOAD_KERNEL
+
+mov si, booting
+call print
+
+mov dl, [bootDriveID]
+mov byte [0x9000], dl
+
+mov di, 0xA000
+mov eax, 0xE820
+xor ebx, ebx
+mov ecx, 24
+mov edx, 0x534D4150
+int 0x15
+
+jc BIOS_NO_MEM
+cmp eax, 0x534D4150
+jnz BIOS_NO_MEM
+cmp cl, 20
+jnz BIOS_MEM_NO20
+
+MEM_L:
+    test ebx, ebx
+    jz MEM_FINISHED
+
+    mov ax, di
+    xor ch, ch
+    add ax, cx
+    mov di, ax
+
+    mov eax, 0xE820
+    mov ecx, 24
+    int 0x15
+    jmp MEM_L
+
+MEM_FINISHED:
+    mov eax, cr0
+    or al, 1
+    mov cr0, eax
+
+    mov ax, 0x08
+    mov ds, ax
+    mov es, ax
+    mov fs, ax
+    mov gs, ax
+    mov ss, ax
+
+; Infomation: https://wiki.osdev.org/ELF
+mov ebx, 0x20002C
+mov dl, byte [ebx]
+push dx
+
+mov ebx, 0x20001C
+mov ebx, dword [ebx]
+push ebx
+
+xor dh, dh
+PHT:
+    xor eax, eax
+    mov al, dh
+    shl eax, 5
+    add eax, dword [esp]
+    add eax, 0x200000
+    mov ebx, eax
+
+    mov eax, dword [ebx]
+    cmp eax, 1
+    jnz .PHT_ignore
+
+    add ebx, 4
+    mov eax, dword [ebx]
+    add eax, 0x200000
+    push eax
+
+    add ebx, 4
+    mov eax, dword [ebx]
+    push eax
+
+    add ebx, 8
+    mov eax, dword [ebx]
+    push eax
+
+    push dx
+    xor ecx, ecx
+    .MOVE_KERNEL_L:
+        mov ebx, [esp+10]
+        add ebx, ecx
+        mov ebx, dword [ebx]
+
+        mov eax, ebx
+        mov ebx, dword [esp+6]
+        add ebx, ecx
+        mov dword [ebx], eax
+
+        add ecx, 4
+        cmp ecx, dword [esp+2]
+        jl .MOVE_KERNEL_L
+    pop dx
+    add esp, 12
+
+    .PHT_ignore:
+        inc dh
+        cmp dh, dl
+        jl PHT
+
+jmp (codedesc - gdt):protectedMode
+
+protectedMode:
+    BITS 32
+    mov ebx, 0x100000
+    mov eax, dword [ebx]
+    jmp eax
+
+BITS 16
+BIOS_NO_MEM:
+    mov si, nomem
+    call print
+    jmp $
+BIOS_MEM_NO20:
+    mov si, memno20
+    call print
+    jmp $
+
+gdtinfo:
+    dw gdt_end - gdt - 1
+    dd gdt
+    gdt dd 0, 0
+    flatdesc db 0xff, 0xff, 0, 0, 0, 10010010b, 11001111b, 0
+    codedesc db 0xff, 0xff, 0, 0, 0, 10011010b, 11001111b, 0
+    gdt_end:
+
+lba_not_supported:
+    mov si, nolba
+    call print
+    jmp $
+
+boot db "BOOT"
+boot_len equ ($ - boot)
+kernelbin db "KERNEL.BIN", 0x3B, "1"
+kernelbin_len equ ($ - kernelbin)
\ No newline at end of file
diff --git a/src/bootloader/stage1.asm b/src/bootloader/stage1.asm
deleted file mode 100644
index 2f62a4d..0000000
--- a/src/bootloader/stage1.asm
+++ /dev/null
@@ -1,73 +0,0 @@
-org 0x7c00
-bits 16
-
-STAGE2 equ 0x800
-STAGE2_SECTORS equ 2+1
-TRACKS equ 2
-mov [BOOT_DRIVE], dl
-
-mov bp, 0x9000
-mov sp, bp
-
-start:
-    jmp load_stage2
-
-print:
-    lodsb
-    or al, al
-    jz done
-    mov ah, 0eh
-    int 10h
-    jmp print
-done:
-    ret
-
-clear:
-    pusha
-    mov ah, 0x00
-    mov al, 0x03
-    int 0x10
-    popa
-    ret
-
-print_message:
-    xor ax, ax
-    mov ds, ax
-    mov es, ax
-
-    call clear
-    mov si, msg
-    call print
-
-disk_load:
-    pusha
-    push dx
-    mov ah, 0x02
-    mov al, dh
-    mov dh, 0x0
-    int 0x13
-    popa
-    ret
-
-load_stage2:
-    call print_message
-    mov cl, 2
-    mov bx, STAGE2
-    mov dh, 1
-    mov dl, [BOOT_DRIVE]
-load_sector:
-    call disk_load
-    cmp cl, STAGE2_SECTORS
-    je loaded
-    cmp cl, 15
-    add cl, 1
-    add bx, 512
-    jmp load_sector
-loaded:
-    ret
-
-msg db "Booting Melvix...", 0
-BOOT_DRIVE db 0
-
-times 510 - ($-$$) db 0
-dw 0xAA55
diff --git a/src/bootloader/stage2.asm b/src/bootloader/stage2.asm
deleted file mode 100644
index e49eee7..0000000
--- a/src/bootloader/stage2.asm
+++ /dev/null
@@ -1,109 +0,0 @@
-org 0x800
-bits 16
-
-KERNEL equ 0x1000
-KERNEL_SECTORS equ 24
-
-call load_kernel
-
-int 0x12
-mov [0x600], ax
-
-call switch_to_pm
-
-gtd_start:
-gdt_null:
-    dd 0x0
-    dd 0x0
-
-gdt_code:
-    dw 0xffff
-    dw 0x0
-    db 0x0
-    db 10011010b
-    db 11001111b
-    db 0x0
-
-gdt_data:
-    dw 0xffff
-    dw 0x0
-    db 0x0
-    db 10010010b
-    db 11001111b
-    db 0x0
-gdt_end:
-
-gdt_descriptor:
-    dw gdt_end - gdt_start - 1
-    dd gdt_start
-
-CODE_SEG equ gdt_code - gdt_start
-DATA_SEG equ gdt_data - gdt_start
-
-switch_to_pm:
-    cli
-    lgdt [gdt_descriptor]
-    mov eax, cr0
-    or eax, 0x1
-    mov cr0, eax
-    jmp CODE_SEG:init_pm
-
-bits 32
-init_pm:
-    mov ax, DATA_SEG
-    mov ds, ax
-    mov ss, ax
-    mov es, ax
-    mov fs, ax
-    mov gs, ax
-
-    mov ebp, 0x90000
-    mov esp, 0x90000
-    call begin_pm
-
-load_kernel:
-    mov ax, 3
-    mov cl, 4
-    mov ch, 0
-    mov bx, KERNEL
-    mov dl, [BOOT_DRIVE]
-    mov dh, 0
-    mov ch, 0
-load_sector:
-    mov ah, 0x02
-    mov al, 1
-    int 0x13
-    push bx
-    mov bl, [Sector]
-    cmp bl, KERNEL_SECTORS
-    pop bx
-    je loaded
-    push bx
-    mov bl, [Sector]
-    inc bl
-    mov [Sector], bl
-    pop bx
-    inc cl
-    cmp cl, 18
-    jne continue
-    add ch, 1
-    add ch, 1
-    mov cl, 1
-continue:
-    add bx, BytesPerSector
-    jmp load_sector
-loaded:
-    ret
-
-begin_pm:
-    call KERNEL
-    jmp $
-
-BytesPerSector equ 512
-NumHeads equ 2
-SectorsPerTrack equ 18
-Sector db 0
-
-BOOT_DRIVE db 0
-
-times 1024-($-$$) db 0
diff --git a/src/kernel/boot.asm b/src/kernel/boot.asm
index 0f967b7..b1f6018 100644
--- a/src/kernel/boot.asm
+++ b/src/kernel/boot.asm
@@ -1,63 +1,60 @@
-; The first section of the ELF will be used to locate the entry point.
-section .ezlocation
-dd _start
+section .start_location
+    dd _start
 
-; Set stack
+; Initialize stack
 section .bss
-align 16
-global STACK_BOTTOM
-global STACK_TOP
-
-STACK_BOTTOM:
-resb 0x4000
-STACK_TOP:
+    align 16
+    global STACK_BOTTOM
+    global STACK_TOP
 
+    STACK_BOTTOM:
+        resb 0x4000
+    STACK_TOP:
 
 section .text
+    global _start
+    extern kernel_main
+    _start:
+        mov esp, STACK_TOP
+        push ebx
+        push eax
 
-global _start
-extern kernel_main
-_start:
-	mov esp, STACK_TOP
-	push ebx
-	push eax
-
-	call kernel_main
-	cli
+        call kernel_main
+        cli
 
-hlt_L:
-	hlt
-	jmp hlt_L
+    hlt_L:
+        hlt
+        jmp hlt_L
 
-%include "src/kernel/gdt/gdt.asm"
+    %include "src/kernel/gdt/gdt.asm"
 
-%include "src/kernel/interrupts/idt.asm"
+    %include "src/kernel/interrupts/idt.asm"
 
-%include "src/kernel/interrupts/isr.asm"
+    %include "src/kernel/interrupts/isr.asm"
 
-%include "src/kernel/interrupts/irq.asm"
+    %include "src/kernel/interrupts/irq.asm"
 
-%include "src/kernel/interact.asm"
+    %include "src/kernel/interact.asm"
 
-global switch_to_user
-extern test_user
-switch_to_user:
-    sti
-    mov ax, 0x23
-    mov ds, ax
-    mov es, ax
-    mov fs, ax
-    mov gs, ax
+    global switch_to_user
+    extern test_user
+    switch_to_user:
+        sti
+        mov ax, 0x23
+        mov ds, ax
+        mov es, ax
+        mov fs, ax
+        mov gs, ax
 
-    mov eax, esp
-    push 0x23
-    push eax
-    pushf
-    push 0x1B
-    push test_user
-    iret
+        mov eax, esp
+        push 0x23
+        push eax
+        pushf
+        push 0x1B
+        push test_user
+        iret
 
 section .sizedetect
-global ASM_KERNEL_END
-ASM_KERNEL_END:
-	; Kernel size detection
+    global ASM_KERNEL_END
+    ASM_KERNEL_END:
+        ; Kernel size detection
diff --git a/src/kernel/kernel.c b/src/kernel/kernel.c
index 214ecf0..70f9d60 100644
--- a/src/kernel/kernel.c
+++ b/src/kernel/kernel.c
@@ -6,14 +6,13 @@
 #include <kernel/paging/paging.h>
 #include <kernel/input/input.h>
 #include <kernel/acpi/acpi.h>
-#include <kernel/mutliboot.h>
 #include <kernel/fs/initrd.h>
 #include <kernel/syscall/syscall.h>
 #include <kernel/smbios/smbios.h>
 
 extern void switch_to_user();
 
-void kernel_main(struct multiboot *mboot_ptr) {
+void kernel_main() {
     vga_log("Installing basic features of Melvix...", 0);
     // Install features
     timer_install();
diff --git a/src/kernel/linker.ld b/src/kernel/linker.ld
index 9062b27..8ca4406 100644
--- a/src/kernel/linker.ld
+++ b/src/kernel/linker.ld
@@ -1,27 +1,22 @@
 ENTRY(_start)
 
-/* Where the sections of the object files will be put in the final image */
 SECTIONS {
 	/* Begin @ 1 MB */
 	. = 1M;
 
-	/* Put the multiboot header. Next, the .text section. */
 	.text BLOCK(4K) : ALIGN(4K) {
-		*(.ezlocation)
+		*(.start_location)
 		*(.text)
 	}
 
-	/* Read-only data. */
 	.rodata BLOCK(4K) : ALIGN(4K) {
 		*(.rodata)
 	}
 
-	/* Read-write data (initialized) */
 	.data BLOCK(4K) : ALIGN(4K) {
 		*(.data)
 	}
 
-	/* Read-write data (uninitialized) and stack */
 	.bss BLOCK(4K) : ALIGN(4K) {
 		*(COMMON)
 		*(.bss)
diff --git a/src/kernel/mutliboot.h b/src/kernel/mutliboot.h
deleted file mode 100644
index 683f95f..0000000
--- a/src/kernel/mutliboot.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#ifndef MELVIX_MUTLIBOOT_H
-#define MELVIX_MUTLIBOOT_H
-
-#include <stdint.h>
-
-#define MULTIBOOT_FLAG_MEM     0x001
-#define MULTIBOOT_FLAG_DEVICE  0x002
-#define MULTIBOOT_FLAG_CMDLINE 0x004
-#define MULTIBOOT_FLAG_MODS    0x008
-#define MULTIBOOT_FLAG_AOUT    0x010
-#define MULTIBOOT_FLAG_ELF     0x020
-#define MULTIBOOT_FLAG_MMAP    0x040
-#define MULTIBOOT_FLAG_CONFIG  0x080
-#define MULTIBOOT_FLAG_LOADER  0x100
-#define MULTIBOOT_FLAG_APM     0x200
-#define MULTIBOOT_FLAG_VBE     0x400
-
-struct multiboot {
-    uint32_t flags;
-    uint32_t mem_lower;
-    uint32_t mem_upper;
-    uint32_t boot_device;
-    uint32_t cmdline;
-    uint32_t mods_count;
-    uint32_t mods_addr;
-    uint32_t num;
-    uint32_t size;
-    uint32_t addr;
-    uint32_t shndx;
-    uint32_t mmap_length;
-    uint32_t mmap_addr;
-    uint32_t drives_length;
-    uint32_t drives_addr;
-    uint32_t config_table;
-    uint32_t boot_loader_name;
-    uint32_t apm_table;
-    uint32_t vbe_control_info;
-    uint32_t vbe_mode_info;
-    uint32_t vbe_mode;
-    uint32_t vbe_interface_seg;
-    uint32_t vbe_interface_off;
-    uint32_t vbe_interface_len;
-}  __attribute__((packed));
-
-typedef struct multiboot_header multiboot_header_t;
-
-#endif
diff --git a/src/kernel/system.c b/src/kernel/system.c
index 688b0b1..303fc83 100644
--- a/src/kernel/system.c
+++ b/src/kernel/system.c
@@ -5,7 +5,15 @@
 
 char *vga_buffer = (char *) 0x500;
 
+void vga_clear() {
+    uint16_t *terminal_buffer = (uint16_t *) 0xB8000;
+    for (size_t y = 0; y < 25; y++)
+        for (size_t x = 0; x < 80; x++)
+            terminal_buffer[y * 80 + x] = 0 | (uint16_t) 0x700;
+}
+
 void vga_log(char *msg, int line) {
+    if (line == 0) vga_clear();
     uint16_t *terminal_buffer = (uint16_t *) 0xB8000;
     for (size_t i = 0; i < strlen(msg); i++)
         terminal_buffer[line * 80 + i] = (uint16_t) msg[i] | (uint16_t) 0x700;