mm.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621
  1. // SPDX-License-Identifier: LGPL-2.1 OR BSD-3-Clause
  2. #include <stdio.h>
  3. #include <stdlib.h>
  4. #include "mm.h"
  5. #ifdef MM_DEBUG
  6. #ifdef _MSC_VER
  7. #define DPRINT(_fmt, ...) printf(_fmt, __VA_ARGS__)
  8. #else
  9. #define DPRINT(_fmt, ...) printf(_fmt, args...)
  10. #endif
  11. #else
  12. #define DPRINT(_fmt, ...)
  13. #endif
  14. #define P_ALLOC(_x) malloc(_x)
  15. #define P_FREE(_x) free(_x)
  16. #define ASSERT(_exp) if (!(_exp)) { printf("ASSERT at %s:%d\n", __FILE__, __LINE__); while(1); }
  17. #define HEIGHT(_tree) (_tree==NULL ? -1 : _tree->height)
  18. #define MAX(_a, _b) (_a >= _b ? _a : _b)
  19. typedef enum {
  20. LEFT,
  21. RIGHT
  22. } rotation_dir_t;
  23. typedef struct avl_node_data_struct {
  24. int key;
  25. page_t* page;
  26. } avl_node_data_t;
  27. static avl_node_t*
  28. make_avl_node(
  29. vmem_key_t key,
  30. page_t* page
  31. )
  32. {
  33. avl_node_t* node = (avl_node_t*)P_ALLOC(sizeof(avl_node_t));
  34. node->key = key;
  35. node->page = page;
  36. node->height = 0;
  37. node->left = NULL;
  38. node->right = NULL;
  39. return node;
  40. }
  41. static int
  42. get_balance_factor(
  43. avl_node_t* tree
  44. )
  45. {
  46. int factor = 0;
  47. if (tree) {
  48. factor = HEIGHT(tree->right) - HEIGHT(tree->left);
  49. }
  50. return factor;
  51. }
  52. /*
  53. * Left Rotation
  54. *
  55. * A B
  56. * \ / \
  57. * B => A C
  58. * / \ \
  59. * D C D
  60. *
  61. */
  62. static avl_node_t*
  63. rotation_left(
  64. avl_node_t* tree
  65. )
  66. {
  67. avl_node_t* rchild;
  68. avl_node_t* lchild;
  69. if (tree == NULL) return NULL;
  70. rchild = tree->right;
  71. if (rchild == NULL) {
  72. return tree;
  73. }
  74. lchild = rchild->left;
  75. rchild->left = tree;
  76. tree->right = lchild;
  77. tree->height = MAX(HEIGHT(tree->left), HEIGHT(tree->right)) + 1;
  78. rchild->height = MAX(HEIGHT(rchild->left), HEIGHT(rchild->right)) + 1;
  79. return rchild;
  80. }
  81. /*
  82. * Reft Rotation
  83. *
  84. * A B
  85. * \ / \
  86. * B => D A
  87. * / \ /
  88. * D C C
  89. *
  90. */
  91. static avl_node_t*
  92. rotation_right(
  93. avl_node_t* tree
  94. )
  95. {
  96. avl_node_t* rchild;
  97. avl_node_t* lchild;
  98. if (tree == NULL) return NULL;
  99. lchild = tree->left;
  100. if (lchild == NULL) return NULL;
  101. rchild = lchild->right;
  102. lchild->right = tree;
  103. tree->left = rchild;
  104. tree->height = MAX(HEIGHT(tree->left), HEIGHT(tree->right)) + 1;
  105. lchild->height = MAX(HEIGHT(lchild->left), HEIGHT(lchild->right)) + 1;
  106. return lchild;
  107. }
  108. static avl_node_t*
  109. do_balance(
  110. avl_node_t* tree
  111. )
  112. {
  113. int bfactor = 0, child_bfactor; /* balancing factor */
  114. bfactor = get_balance_factor(tree);
  115. if (bfactor >= 2) {
  116. child_bfactor = get_balance_factor(tree->right);
  117. if (child_bfactor == 1 || child_bfactor == 0) {
  118. tree = rotation_left(tree);
  119. } else if (child_bfactor == -1) {
  120. tree->right = rotation_right(tree->right);
  121. tree = rotation_left(tree);
  122. } else {
  123. fprintf(stderr, "invalid balancing factor: %d\n", child_bfactor);
  124. ASSERT(0);
  125. return NULL;
  126. }
  127. } else if (bfactor <= -2) {
  128. child_bfactor = get_balance_factor(tree->left);
  129. if (child_bfactor == -1 || child_bfactor == 0) {
  130. tree = rotation_right(tree);
  131. } else if (child_bfactor == 1) {
  132. tree->left = rotation_left(tree->left);
  133. tree = rotation_right(tree);
  134. } else {
  135. fprintf(stderr, "invalid balancing factor: %d\n", child_bfactor);
  136. ASSERT(0);
  137. return NULL;
  138. }
  139. }
  140. return tree;
  141. }
  142. static avl_node_t*
  143. unlink_end_node(
  144. avl_node_t* tree,
  145. int dir,
  146. avl_node_t** found_node
  147. )
  148. {
  149. *found_node = NULL;
  150. if (tree == NULL) return NULL;
  151. if (dir == LEFT) {
  152. if (tree->left == NULL) {
  153. *found_node = tree;
  154. return NULL;
  155. }
  156. } else {
  157. if (tree->right == NULL) {
  158. *found_node = tree;
  159. return NULL;
  160. }
  161. }
  162. if (dir == LEFT) {
  163. tree->left = unlink_end_node(tree->left, LEFT, found_node);
  164. if (tree->left == NULL) {
  165. tree->left = (*found_node)->right;
  166. (*found_node)->left = NULL;
  167. (*found_node)->right = NULL;
  168. }
  169. } else {
  170. tree->right = unlink_end_node(tree->right, RIGHT, found_node);
  171. if (tree->right == NULL) {
  172. tree->right = (*found_node)->left;
  173. (*found_node)->left = NULL;
  174. (*found_node)->right = NULL;
  175. }
  176. }
  177. tree->height = MAX(HEIGHT(tree->left), HEIGHT(tree->right)) + 1;
  178. return do_balance(tree);
  179. }
  180. static avl_node_t*
  181. avltree_insert(
  182. avl_node_t* tree,
  183. vmem_key_t key,
  184. page_t* page
  185. )
  186. {
  187. if (tree == NULL) {
  188. tree = make_avl_node(key, page);
  189. } else {
  190. if (key >= tree->key) {
  191. tree->right = avltree_insert(tree->right, key, page);
  192. } else {
  193. tree->left = avltree_insert(tree->left, key, page);
  194. }
  195. }
  196. tree = do_balance(tree);
  197. tree->height = MAX(HEIGHT(tree->left), HEIGHT(tree->right)) + 1;
  198. return tree;
  199. }
  200. static avl_node_t*
  201. do_unlink(
  202. avl_node_t* tree
  203. )
  204. {
  205. avl_node_t* node;
  206. avl_node_t* end_node;
  207. node = unlink_end_node(tree->right, LEFT, &end_node);
  208. if (node) {
  209. tree->right = node;
  210. } else {
  211. node = unlink_end_node(tree->left, RIGHT, &end_node);
  212. if (node) tree->left = node;
  213. }
  214. if (node == NULL) {
  215. node = tree->right ? tree->right : tree->left;
  216. end_node = node;
  217. }
  218. if (end_node) {
  219. end_node->left = (tree->left != end_node) ? tree->left : end_node->left;
  220. end_node->right = (tree->right != end_node) ? tree->right : end_node->right;
  221. end_node->height = MAX(HEIGHT(end_node->left), HEIGHT(end_node->right)) + 1;
  222. }
  223. tree = end_node;
  224. return tree;
  225. }
  226. static avl_node_t*
  227. avltree_remove(
  228. avl_node_t* tree,
  229. avl_node_t** found_node,
  230. vmem_key_t key
  231. )
  232. {
  233. *found_node = NULL;
  234. if (tree == NULL) {
  235. DPRINT("failed to find key %d\n", (int)key);
  236. return NULL;
  237. }
  238. if (key == tree->key) {
  239. *found_node = tree;
  240. tree = do_unlink(tree);
  241. } else if (key > tree->key) {
  242. tree->right = avltree_remove(tree->right, found_node, key);
  243. } else {
  244. tree->left = avltree_remove(tree->left, found_node, key);
  245. }
  246. if (tree) tree->height = MAX(HEIGHT(tree->left), HEIGHT(tree->right)) + 1;
  247. tree = do_balance(tree);
  248. return tree;
  249. }
  250. void
  251. avltree_free(
  252. avl_node_t* tree
  253. )
  254. {
  255. if (tree == NULL) return;
  256. if (tree->left == NULL && tree->right == NULL) {
  257. P_FREE(tree);
  258. return;
  259. }
  260. avltree_free(tree->left);
  261. tree->left = NULL;
  262. avltree_free(tree->right);
  263. tree->right = NULL;
  264. P_FREE(tree);
  265. }
  266. static avl_node_t*
  267. remove_approx_value(
  268. avl_node_t* tree,
  269. avl_node_t** found,
  270. vmem_key_t key
  271. )
  272. {
  273. *found = NULL;
  274. if (tree == NULL) {
  275. return NULL;
  276. }
  277. if (key == tree->key) {
  278. *found = tree;
  279. tree = do_unlink(tree);
  280. } else if (key > tree->key) {
  281. tree->right = remove_approx_value(tree->right, found, key);
  282. } else {
  283. tree->left = remove_approx_value(tree->left, found, key);
  284. if (*found == NULL) {
  285. *found = tree;
  286. tree = do_unlink(tree);
  287. }
  288. }
  289. if (tree) tree->height = MAX(HEIGHT(tree->left), HEIGHT(tree->right)) + 1;
  290. tree = do_balance(tree);
  291. return tree;
  292. }
  293. static void
  294. set_blocks_free(
  295. video_mm_t *mm,
  296. int pageno,
  297. int npages
  298. )
  299. {
  300. int last_pageno = pageno + npages - 1;
  301. int i;
  302. page_t* page;
  303. page_t* last_page;
  304. ASSERT(npages);
  305. if (last_pageno >= mm->num_pages) {
  306. DPRINT("set_blocks_free: invalid last page number: %d\n", last_pageno);
  307. ASSERT(0);
  308. return;
  309. }
  310. for (i=pageno; i<=last_pageno; i++) {
  311. mm->page_list[i].used = 0;
  312. mm->page_list[i].alloc_pages = 0;
  313. mm->page_list[i].first_pageno = -1;
  314. }
  315. page = &mm->page_list[pageno];
  316. page->alloc_pages = npages;
  317. last_page = &mm->page_list[last_pageno];
  318. last_page->first_pageno = pageno;
  319. mm->free_tree = avltree_insert(mm->free_tree, MAKE_KEY(npages, pageno), page); /*lint !e571 Suspicious cast*/
  320. }
  321. static void
  322. set_blocks_alloc(
  323. video_mm_t *mm,
  324. int pageno,
  325. int npages
  326. )
  327. {
  328. int last_pageno = pageno + npages - 1;
  329. int i;
  330. page_t* page;
  331. page_t* last_page;
  332. if (last_pageno >= mm->num_pages) {
  333. DPRINT("set_blocks_free: invalid last page number: %d\n", last_pageno);
  334. ASSERT(0);
  335. return;
  336. }
  337. for (i=pageno; i<=last_pageno; i++) {
  338. mm->page_list[i].used = 1;
  339. mm->page_list[i].alloc_pages = 0;
  340. mm->page_list[i].first_pageno = -1;
  341. }
  342. page = &mm->page_list[pageno];
  343. page->alloc_pages = npages;
  344. last_page = &mm->page_list[last_pageno];
  345. last_page->first_pageno = pageno;
  346. mm->alloc_tree = avltree_insert(mm->alloc_tree, MAKE_KEY(page->addr, 0), page);
  347. }
  348. int
  349. vmem_init(
  350. video_mm_t* mm,
  351. unsigned long addr,
  352. unsigned long size
  353. )
  354. {
  355. int i;
  356. mm->base_addr = (addr+(VMEM_PAGE_SIZE-1))&~(VMEM_PAGE_SIZE-1);
  357. mm->mem_size = size&~VMEM_PAGE_SIZE;
  358. mm->num_pages = mm->mem_size/VMEM_PAGE_SIZE;
  359. mm->page_list = (page_t*)P_ALLOC(mm->num_pages*sizeof(page_t));
  360. mm->free_tree = NULL;
  361. mm->alloc_tree = NULL;
  362. mm->free_page_count = mm->num_pages;
  363. mm->alloc_page_count = 0;
  364. for (i=0; i<mm->num_pages; i++) {
  365. mm->page_list[i].pageno = i;
  366. mm->page_list[i].addr = mm->base_addr + i*VMEM_PAGE_SIZE;
  367. mm->page_list[i].alloc_pages = 0;
  368. mm->page_list[i].used = 0;
  369. mm->page_list[i].first_pageno = -1;
  370. }
  371. set_blocks_free(mm, 0, mm->num_pages);
  372. return 0;
  373. }
  374. int
  375. vmem_exit(
  376. video_mm_t* mm
  377. )
  378. {
  379. if (mm == NULL) {
  380. DPRINT("vmem_exit: invalid handle\n");
  381. return -1;
  382. }
  383. if (mm->free_tree) {
  384. avltree_free(mm->free_tree);
  385. mm->free_tree = NULL;
  386. }
  387. if (mm->alloc_tree) {
  388. avltree_free(mm->alloc_tree);
  389. mm->alloc_tree = NULL;
  390. }
  391. if (mm->page_list) {
  392. P_FREE(mm->page_list);
  393. mm->page_list = NULL;
  394. }
  395. return 0;
  396. }
  397. unsigned long
  398. vmem_alloc(
  399. video_mm_t* mm,
  400. int size,
  401. unsigned long pid
  402. )
  403. {
  404. avl_node_t* node;
  405. page_t* free_page;
  406. int npages, free_size;
  407. int alloc_pageno;
  408. unsigned long ptr;
  409. if (mm == NULL) {
  410. DPRINT("vmem_alloc: invalid handle\n");
  411. return (unsigned long)-1;
  412. }
  413. if (size <= 0) return (unsigned long)-1;
  414. npages = (size + VMEM_PAGE_SIZE -1)/VMEM_PAGE_SIZE;
  415. mm->free_tree = remove_approx_value(mm->free_tree, &node, MAKE_KEY(npages, 0)); /*lint !e571 Suspicious cast*/
  416. if (node == NULL) {
  417. return (unsigned long)-1;
  418. }
  419. free_page = node->page;
  420. free_size = KEY_TO_VALUE(node->key);
  421. alloc_pageno = free_page->pageno;
  422. set_blocks_alloc(mm, alloc_pageno, npages);
  423. if (npages != free_size) {
  424. int free_pageno = alloc_pageno + npages;
  425. set_blocks_free(mm, free_pageno, (free_size-npages));
  426. }
  427. P_FREE(node);
  428. ptr = mm->page_list[alloc_pageno].addr;
  429. mm->alloc_page_count += npages;
  430. mm->free_page_count -= npages;
  431. return ptr;
  432. }
  433. int
  434. vmem_free(
  435. video_mm_t* mm,
  436. unsigned long ptr,
  437. unsigned long pid
  438. )
  439. {
  440. unsigned long addr;
  441. avl_node_t* found;
  442. page_t* page;
  443. int pageno, prev_free_pageno, next_free_pageno;
  444. int prev_size, next_size;
  445. int merge_page_no, merge_page_size, free_page_size;
  446. if (mm == NULL) {
  447. DPRINT("vmem_free: invalid handle\n");
  448. return -1;
  449. }
  450. addr = ptr;
  451. //printf("FREE: 0x%08x\n", addr);
  452. mm->alloc_tree = avltree_remove(mm->alloc_tree, &found, MAKE_KEY(addr, 0));
  453. if (found == NULL) {
  454. DPRINT("vmem_free: 0x%08x not found\n", addr);
  455. return -1;
  456. }
  457. /* find previous free block */
  458. page = found->page;
  459. pageno = page->pageno;
  460. free_page_size = page->alloc_pages;
  461. prev_free_pageno = pageno-1;
  462. prev_size = -1;
  463. if (prev_free_pageno >= 0) {
  464. if (mm->page_list[prev_free_pageno].used == 0) {
  465. prev_free_pageno = mm->page_list[prev_free_pageno].first_pageno;
  466. prev_size = mm->page_list[prev_free_pageno].alloc_pages;
  467. }
  468. }
  469. /* find next free block */
  470. next_free_pageno = pageno + page->alloc_pages;
  471. next_free_pageno = (next_free_pageno == mm->num_pages) ? -1 : next_free_pageno;
  472. next_size = -1;
  473. if (next_free_pageno >= 0) {
  474. if (mm->page_list[next_free_pageno].used == 0) {
  475. next_size = mm->page_list[next_free_pageno].alloc_pages;
  476. }
  477. }
  478. P_FREE(found);
  479. /* merge */
  480. merge_page_no = page->pageno;
  481. merge_page_size = page->alloc_pages;
  482. if (prev_size >= 0) {
  483. mm->free_tree = avltree_remove(mm->free_tree, &found, MAKE_KEY(prev_size, prev_free_pageno)); /*lint !e571 Suspicious cast*/
  484. if (found == NULL) {
  485. ASSERT(0);
  486. return -1;
  487. }
  488. merge_page_no = found->page->pageno;
  489. merge_page_size += found->page->alloc_pages;
  490. P_FREE(found);
  491. }
  492. if (next_size >= 0) {
  493. mm->free_tree = avltree_remove(mm->free_tree, &found, MAKE_KEY(next_size, next_free_pageno)); /*lint !e571 Suspicious cast*/
  494. if (found == NULL) {
  495. ASSERT(0);
  496. return -1;
  497. }
  498. merge_page_size += found->page->alloc_pages;
  499. P_FREE(found);
  500. }
  501. page->alloc_pages = 0;
  502. page->first_pageno = -1;
  503. set_blocks_free(mm, merge_page_no, merge_page_size);
  504. mm->alloc_page_count -= free_page_size;
  505. mm->free_page_count += free_page_size;
  506. DPRINT("FREE: total(%d) alloc(%d) free(%d)\n", mm->num_pages, mm->alloc_page_count, mm->free_page_count);
  507. return 0;
  508. }
  509. int
  510. vmem_get_info(
  511. video_mm_t* mm,
  512. vmem_info_t* info
  513. )
  514. {
  515. if (mm == NULL) {
  516. //DPRINT("vmem_get_info: invalid handle\n");
  517. return -1;
  518. }
  519. if (info == NULL) {
  520. return -1;
  521. }
  522. info->total_pages = mm->num_pages;
  523. info->alloc_pages = mm->alloc_page_count;
  524. info->free_pages = mm->free_page_count;
  525. info->page_size = VMEM_PAGE_SIZE;
  526. return 0;
  527. }