phoenix-rtos-kernel/programs.c at master · jsarzy/phoenix-rtos-kernel · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
/*
 * Phoenix-RTOS
 *
 * Operating system kernel
 *
 * Empty file holding programs
 *
 * Copyright 2018 Phoenix Systems
 * Author: Pawel Pisarczyk
 *
 * This file is part of Phoenix-RTOS.
 *
 * %LICENSE%
 */

#include HAL
#include "vm/vm.h"
#include "programs.h"

#include "include/errno.h"


#ifndef CPIO_PAD
#define CPIO_PAD 0x3
#endif

extern char programs[];

unsigned int programs_a2i(char *s)
{
	unsigned int i, k = 28, v = 0;
	char d;

	for (i = 0; (i < 8) && s[i]; i++) {
		d = s[i] - '0';

		if ((d > 16) && (d < 23))
			d -= 7;
		else if ((d > 48) && (d < 55))
			d -= 39;
		else if (d > 9)
			return -EINVAL;

		v += (d << k);
		k -= 4;
	}
	return v;
}


int programs_decode(vm_map_t *kmap, vm_object_t *kernel)
{
#if !defined(CPU_STM32) && !defined(CPU_IMXRT)
	cpio_newc_t *cpio = (void *)programs;
	unsigned int fs, ns, sz, k;
	page_t *p;
	void *vaddr;
	syspage_program_t *pr;

	if (hal_strncmp(cpio->c_magic, "070701", 6)) {

		/* Happens in QEMU when programs are not page aligned. What the hell? */
		if (!hal_strcmp(HAL, "hal/ia32/hal.h"))
			cpio = (void *)(programs - 0x1000);

		if (hal_strncmp(cpio->c_magic, "070701", 6)) {
			lib_printf("programs: valid cpio archive not found\n");
			return -EINVAL;
		}

		lib_printf("programs: cpio archive found dislocated\n");
	}

	for (;;) {
		if (!hal_strcmp(cpio->name, "TRAILER!!!"))
			break;

		pr = &syspage->progs[syspage->progssz];

		/* Initialize cmdline */
		k = hal_strlen((char *)cpio->name);
		while (--k && ((char *)cpio->name)[k - 1] != '/');

		hal_memset(pr->cmdline, 0, sizeof(pr->cmdline));
		hal_strncpy(pr->cmdline, (char *)cpio->name + k, sizeof(pr->cmdline) - 1);

		fs = programs_a2i(cpio->c_filesize);
		if (fs == -EINVAL) {
			lib_printf("programs: invalid filesize\n");
			return -EINVAL;
		}
		ns = programs_a2i(cpio->c_namesize);
		if (ns == -EINVAL) {
			lib_printf("programs: invalid namesize\n");
			return -EINVAL;
		}

		cpio = (void *)(((ptr_t)cpio + sizeof(cpio_newc_t) + ns + CPIO_PAD) & ~CPIO_PAD);

		if (fs) {
			/* Alloc pages for program */
			sz = ((fs + SIZE_PAGE - 1) / SIZE_PAGE) * SIZE_PAGE;
			if ((p = vm_pageAlloc(sz, PAGE_OWNER_APP)) == NULL)
				return -ENOMEM;

			if ((vaddr = vm_mmap(kmap, kmap->start, p, sz, PROT_READ | PROT_WRITE, kernel, -1, MAP_NONE)) == NULL) {
				vm_pageFree(p);
				return -ENOMEM;
			}
			hal_memcpy(vaddr, cpio, fs);
			//vm_munmap(kmap, vaddr, sz);

			pr->start = (typeof(pr->start))p->addr;
			pr->end = (typeof(pr->end))p->addr + fs;
		}

		syspage->progssz++;
		cpio = (void *)(((ptr_t)cpio + fs + CPIO_PAD) & ~CPIO_PAD);
	}

#endif

	return EOK;
}